def execute(self) -> None: # XXX: https://github.com/robotpy/robotpy-wpilib/issues/635 chassis_speeds = ChassisSpeeds() chassis_speeds.vx = self.vx chassis_speeds.omega = self.vz speeds = self.kinematics.toWheelSpeeds(chassis_speeds) left_ff = self.ff_calculator.calculate(speeds.left) right_ff = self.ff_calculator.calculate(speeds.right) if self.open_loop: self.left_front.setVoltage(left_ff) self.right_front.setVoltage(right_ff) else: self.left_pid.setReference(speeds.left, rev.ControlType.kVelocity, arbFeedforward=left_ff) self.right_pid.setReference(speeds.right, rev.ControlType.kVelocity, arbFeedforward=right_ff) self.odometry.update( self._get_heading(), self.left_encoder.getPosition(), self.right_encoder.getPosition(), )
def setup_trajectory(self, trajectory: Trajectory): self.controller = RamseteController() self.left_controller.reset() self.right_controller.reset() self.prev_time = 0 self.speed = DifferentialDriveWheelSpeeds() self.controller.setTolerance( Pose2d(0.05, 0.05, Rotation2d(math.radians(5)))) self.initial_state = self.trajectory.sample(0) speeds = ChassisSpeeds() speeds.vx = self.initial_state.velocity speeds.omega = self.initial_state.velocity * self.initial_state.curvature self.prevSpeeds = self.kinematics.toWheelSpeeds(speeds)
def autonomousPeriodic(self): preChassis = ChassisSpeeds() preChassis.vx = 5.0 preChassis.vy = 0.0 preChassis.omega = 0.0 # Convert to wheel speeds speeds = self.m_kinematics.toWheelSpeeds(preChassis) self.wheelSpeeds = MecanumDriveWheelSpeeds( self.f_l_encoder.getRate(), self.r_l_encoder.getRate(), self.f_r_encoder.getRate(), self.r_r_encoder.getRate(), ) gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle()) # Finally, we can update the pose... self.m_pose = self.MecanumDriveOdometry.update(gyroAngle, self.wheelSpeeds) #For Kinematics, we need to update the wheelspeeds CurrentChassis = self.m_kinematics.toChassisSpeeds(self.wheelSpeeds) print(CurrentChassis) print(self.f_l_encoder.getDistancePerPulse()) print('difference') print(self.f_l_encoder.get() - self.lastCount) print('rate') print(self.r_r_encoder.getRate()) print('lastCount') self.lastCount = self.f_l_encoder.get() print(self.lastCount) ''' left_front = self.feedForward.calculate(speeds.frontLeft)s right_front = self.feedForward.calculate(speeds.frontRight) left_rear = self.feedForward.calculate(speeds.rearLeft) right_rear = self.feedForward.calculate(speeds.rearRight)''' #print(left_front, right_front, left_rear,right_rear) if self.timer.get() < 2.0: # self.drive.driveCartesian(1, 1, 1, 1) #<---- This is using the drive method built into the mecanum dirve. # Maybe we want to use something with more control, like feedforward... '''self.frontLeftMotor.set(-left_front) self.rearLeftMotor.set(right_front) self.frontRightMotor.set(-left_rear) self.rearRightMotor.set(right_rear)''' self.drive.driveCartesian(1, 0, 0, 0) elif self.timer.get() > 2 and self.timer.get() < 4: self.drive.driveCartesian(1, 0, 0, 0) else: self.drive.driveCartesian(0, 0, 0, 0)
def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) self.rstick = wpilib.Joystick(1) self.l_motor = wpilib.Jaguar(1) self.r_motor = wpilib.Jaguar(2) # Position gets automatically updated as robot moves self.gyro = wpilib.AnalogGyro(0) self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor) self.motor = wpilib.Jaguar(4) self.limit1 = wpilib.DigitalInput(1) self.limit2 = wpilib.DigitalInput(2) self.position = wpilib.AnalogInput(2) self.left_encoder = wpilib.Encoder(1, 2) self.right_encoder = wpilib.Encoder(3, 4) self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH) self.chassis_speeds = ChassisSpeeds() self.chassis_speeds.vx = 0.0 self.chassis_speeds.omega = 0.0 if is_sim: self.physics = physics.PhysicsEngine() self.last_tm = time.time()
def update_sim(self, now: float, tm_diff: float): global simulatedDrivetrain for simSpark in simulatedSparks: simSpark.update(tm_diff) if simulatedDrivetrain is not None: #robotMag, robotDir, robotTurn = simulatedDrivetrain.getRobotMovement() robotMag, robotDir, robotTurn, self._drivePositionState = \ simulatedDrivetrain.getRobotPositionOffset(self._drivePositionState, target=False) robotMag /= tm_diff robotTurn /= tm_diff xVel = robotMag * math.cos(robotDir - math.pi / 2) * 0.3048 # 1 ft = 0.3048 m yVel = robotMag * math.sin(robotDir - math.pi / 2) * 0.3048 speeds = ChassisSpeeds(xVel, yVel, robotTurn) #self.physicsController.vector_drive(xVel, yVel, -robotTurn, elapsed) # HACKS: set the time diff to 1 to move by absolute position # increments instead of velocities self.physicsController.drive(speeds, tm_diff) if self.ahrs != None: self.ahrs.angle = math.degrees(self.physicsController.angle)
def test_swerve4_straightline(s4: SwerveDrive4Kinematics): chassisSpeeds = ChassisSpeeds(5, 0, 0) fl, fr, bl, br = s4.toSwerveModuleStates(chassisSpeeds) assert fl.speed == pytest.approx(5.0) assert fr.speed == pytest.approx(5.0) assert bl.speed == pytest.approx(5.0) assert br.speed == pytest.approx(5.0) assert fl.angle.radians() == pytest.approx(0.0) assert fr.angle.radians() == pytest.approx(0.0) assert bl.angle.radians() == pytest.approx(0.0) assert br.angle.radians() == pytest.approx(0.0)
def robotInit(self): # Pull in smart dashboard info... self.sd = NetworkTables.getTable("SmartDashboard") # Start a timer.... self.timer = wpilib.Timer() """Robot initialization function. The Low level is to use the brushless function on the controllers.""" if wpilib.RobotBase.isSimulation(): self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel) self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel) self.frontRightMotor = wpilib.Spark(self.frontRightChannel) self.rearRightMotor = wpilib.Spark(self.rearRightChannel) else: self.frontLeftMotor = rev.CANSparkMax( self.frontLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearLeftMotor = rev.CANSparkMax( self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.frontRightMotor = rev.CANSparkMax( self.frontRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearRightMotor = rev.CANSparkMax( self.rearRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) # The channel on the driver station that the joystick is connected to joystickChannel = 0 m_frontLeftLocation = Translation2d(0.381, 0.381) m_frontRightLocation = Translation2d(0.381, -0.381) m_backLeftLocation = Translation2d(-0.381, 0.381) m_backRightLocation = Translation2d(-0.381, -0.381) # Creating my kinematics object using the wheel locations. self.m_kinematics = MecanumDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) # Example chassis speeds: 1 meter per second forward, 3 meters # per second to the left, and rotation at 1.5 radians per second # counterclockwise. preChassis = ChassisSpeeds() preChassis.vx = 1.0 preChassis.vy = 0.0 preChassis.omega = 0.0 # Convert to wheel speeds wheelSpeeds = MecanumDriveKinematics.toWheelSpeeds(preChassis) # Get the individual wheel speeds frontLeft = wheelSpeeds.frontLeftMetersPerSecond frontRight = wheelSpeeds.frontRightMetersPerSecond backLeft = wheelSpeeds.rearLeftMetersPerSecond backRight = wheelSpeeds.rearRightMetersPerSecond
def setChassisVelocity(self, velocity_x: float, velocity_y: float, velocity_omega) -> None: state = ChassisSpeeds(velocity_x, velocity_y, -velocity_omega) velocity = self.kinematics.toWheelSpeeds(state) self.setWheelVelocity(velocity.left, velocity.right)
def four_motor_swerve_drivetrain( lr_motor: float, rr_motor: float, lf_motor: float, rf_motor: float, lr_angle: float, rr_angle: float, lf_angle: float, rf_angle: float, x_wheelbase=2, y_wheelbase=2, speed=5, deadzone=None, ) -> ChassisSpeeds: """ Four motors that can be rotated in any direction If any motors are inverted, then you will need to multiply that motor's value by -1. :param lr_motor: Left rear motor value (-1 to 1); 1 is forward :param rr_motor: Right rear motor value (-1 to 1); 1 is forward :param lf_motor: Left front motor value (-1 to 1); 1 is forward :param rf_motor: Right front motor value (-1 to 1); 1 is forward :param lr_angle: Left rear motor angle in degrees (0 to 360 measured clockwise from forward position) :param rr_angle: Right rear motor angle in degrees (0 to 360 measured clockwise from forward position) :param lf_angle: Left front motor angle in degrees (0 to 360 measured clockwise from forward position) :param rf_angle: Right front motor angle in degrees (0 to 360 measured clockwise from forward position) :param x_wheelbase: The distance in feet between right and left wheels. :param y_wheelbase: The distance in feet between forward and rear wheels. :param speed: Speed of robot in feet per second (see above) :param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`) :returns: ChassisSpeeds that can be passed to 'drive' .. versionchanged:: 2020.1.0 The output rotation angle was changed from CW to CCW to reflect the current WPILib drivetrain/field objects """ if deadzone: lf_motor = deadzone(lf_motor) lr_motor = deadzone(lr_motor) rf_motor = deadzone(rf_motor) rr_motor = deadzone(rr_motor) # Calculate speed of each wheel lr = lr_motor * speed rr = rr_motor * speed lf = lf_motor * speed rf = rf_motor * speed # Calculate angle in radians lr_rad = math.radians(lr_angle) rr_rad = math.radians(rr_angle) lf_rad = math.radians(lf_angle) rf_rad = math.radians(rf_angle) # Calculate wheelbase radius wheelbase_radius = math.hypot(x_wheelbase / 2.0, y_wheelbase / 2.0) # Calculates the Vx and Vy components # Sin an Cos inverted because forward is 0 on swerve wheels Vx = ((math.sin(lr_rad) * lr) + (math.sin(rr_rad) * rr) + (math.sin(lf_rad) * lf) + (math.sin(rf_rad) * rf)) Vy = ((math.cos(lr_rad) * lr) + (math.cos(rr_rad) * rr) + (math.cos(lf_rad) * lf) + (math.cos(rf_rad) * rf)) # Adjusts the angle corresponding to a diameter that is perpendicular to the radius (add or subtract 45deg) lr_rad = (lr_rad + (math.pi / 4)) % (2 * math.pi) rr_rad = (rr_rad - (math.pi / 4)) % (2 * math.pi) lf_rad = (lf_rad - (math.pi / 4)) % (2 * math.pi) rf_rad = (rf_rad + (math.pi / 4)) % (2 * math.pi) # Finds the rotational velocity by finding the torque and adding them up Vw = wheelbase_radius * ((math.cos(lr_rad) * -lr) + (math.cos(rr_rad) * rr) + (math.cos(lf_rad) * -lf) + (math.cos(rf_rad) * rf)) Vx *= 0.25 Vy *= 0.25 Vw *= 0.25 return ChassisSpeeds.fromFeet(Vx, Vy, Vw)