def __init__(self, kS, kV, trackwidth, trajectory): ''' Creates a controller for following a PathWeaver trajectory. __init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory) :param kS: The kS gain determined by characterizing the Robot's drivetrain :param kV: The kV gain determined by characterizing the Robot's drivetrain :param trackwidth: The horizontal distance between the left and right wheels of the tank drive. :param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand. ''' self.kS = kS self.kV = kV self.trajectory = trajectory self.odometry = DifferentialDriveOdometry( Rotation2d(radians(0)), self.trajectory.initialPose()) self.ramsete = RamseteController(2, 0.7) self.drive_kinematics = DifferentialDriveKinematics(trackwidth) self.are_wheel_speeds_zero = False self.timer = Timer()
def reset(self, chassis, gyro): ''' Re-initializes all of the data in the controller as if the path has not yet been executed. This method MUST be called in teleopInit and autonomousInit directly before the controller is used. reset(self, chassis: traits.DriveTrain, gyro: traits.Gyro) :param chassis: An object that implements the DriveTrain trait. This object's encoders will be reset by this function :param gyro: An object that implements the Gyro trait. This object's angle will be reset by this function ''' # Assert the objects implement the proper traits assert chassis.implements(DriveTrain) assert gyro.implements(Gyro) # The encoders and gyro need to be reset so that the Ramsete # controller is fed data that looks new. # By reseting these sensors, we look like weve never run the path at all! chassis.reset_encoders() gyro.reset() self.are_wheel_speeds_zero = False # The odometry object also needs to be re-initialized # so that it forgets the state from the previous run self.odometry = DifferentialDriveOdometry( Rotation2d(radians(0)), self.trajectory.initialPose()) self.timer.start()
def __init__(self): self.mode = self._Mode.Idle self.odometry = DifferentialDriveOdometry(Rotation2d.fromDegrees(0)) self.desired_output = WheelState() self.desired_velocity = WheelState() self.feedforward = WheelState() self.wheel_left = motorstate.MotorState() self.wheel_right = motorstate.MotorState() self.nt = NetworkTables.getTable(f"/components/chassis")
class Odometry: kinematics: DifferentialDriveKinematics navx: navx.AHRS left_encoder: CANEncoder right_encoder: CANEncoder left_distance = ntproperty('/left distance/', 0) right_distance = ntproperty('/right distance/', 0) def getAngle(self): """Return the navx angle with counter-clockwise as positive""" return -self.navx.getAngle() def setup(self): self.odometry = DifferentialDriveOdometry( Rotation2d(math.radians(self.getAngle()))) def get_pose(self) -> Pose2d: return self.odometry.getPose() @feedback def get_pose_string(self) -> str: pose = self.get_pose() return f'({pose.translation().X()}, {pose.translation().Y()}, {pose.rotation()}' def get_distance(self, left=True): if left: return self.left_distance else: return -self.right_distance def reset(self, new_pose: Pose2d = Pose2d()): self.odometry.resetPosition(new_pose, Rotation2d.fromDegrees(self.getAngle())) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) @property def left_rate(self): return self.left_encoder.getVelocity() @property def right_rate(self): return -self.right_encoder.getVelocity() def execute(self): self.odometry.update(Rotation2d(math.radians(self.getAngle())), self.left_encoder.getPosition(), -self.right_encoder.getPosition()) self.left_distance = self.left_encoder.getPosition() self.right_distance = -self.right_encoder.getPosition()
def setup(self) -> None: self.left_front.setInverted(False) self.right_front.setInverted(True) for motor in ( self.left_front, self.left_rear, self.right_front, self.right_rear, ): motor.setIdleMode(rev.IdleMode.kCoast) self.left_rear.follow(self.left_front) self.right_rear.follow(self.right_front) self.left_encoder = rev.CANEncoder(self.left_front) self.right_encoder = rev.CANEncoder(self.right_front) rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO for enc in (self.left_encoder, self.right_encoder): enc.setPositionConversionFactor(rev_to_m) enc.setVelocityConversionFactor(rev_to_m / 60) self.left_pid: rev.CANPIDController = self.left_front.getPIDController( ) self.right_pid: rev.CANPIDController = self.right_front.getPIDController( ) self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194, kV=2.79, kA=0.457) for pid in (self.left_pid, self.right_pid): # TODO: needs tuning pid.setP(5.19 / 10) pid.setI(0) pid.setD(0) pid.setIZone(0) pid.setFF(0) pid.setOutputRange(-1, 1) self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH) self.odometry = DifferentialDriveOdometry(self._get_heading())
class Chassis: left_rear: rev.CANSparkMax left_front: rev.CANSparkMax right_rear: rev.CANSparkMax right_front: rev.CANSparkMax imu: NavX open_loop = magicbot.tunable(False) vx = magicbot.will_reset_to(0.0) vz = magicbot.will_reset_to(0.0) def setup(self) -> None: self.left_front.setInverted(False) self.right_front.setInverted(True) self.disable_brake_mode() self.left_rear.follow(self.left_front) self.right_rear.follow(self.right_front) self.left_encoder = rev.CANEncoder(self.left_front) self.right_encoder = rev.CANEncoder(self.right_front) rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO for enc in (self.left_encoder, self.right_encoder): enc.setPositionConversionFactor(rev_to_m) enc.setVelocityConversionFactor(rev_to_m / 60) self.left_pid: rev.CANPIDController = self.left_front.getPIDController( ) self.right_pid: rev.CANPIDController = self.right_front.getPIDController( ) self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194, kV=2.79, kA=0.457) for pid in (self.left_pid, self.right_pid): # TODO: needs tuning pid.setP(5.19 / 10) pid.setI(0) pid.setD(0) pid.setIZone(0) pid.setFF(0) pid.setOutputRange(-1, 1) self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH) self.odometry = DifferentialDriveOdometry(self._get_heading()) # default_position on the field self.reset_odometry(Pose2d(-3, 0, Rotation2d(math.pi))) 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 drive(self, vx: float, vz: float) -> None: """Sets the desired robot velocity. Arguments: vx: Forwards linear velocity in m/s. vz: Angular velocity in rad/s, anticlockwise positive. """ self.vx = vx self.vz = vz def disable_closed_loop(self) -> None: """Run the drivetrain in open loop mode (feedforward only).""" self.open_loop = True def enable_closed_loop(self) -> None: """Run the drivetrain in velocity closed loop mode.""" self.open_loop = False def enable_brake_mode(self) -> None: for motor in ( self.left_front, self.left_rear, self.right_front, self.right_rear, ): motor.setIdleMode(rev.IdleMode.kBrake) def disable_brake_mode(self) -> None: for motor in ( self.left_front, self.left_rear, self.right_front, self.right_rear, ): motor.setIdleMode(rev.IdleMode.kCoast) def _get_heading(self) -> Rotation2d: """Get the current heading of the robot from the IMU, anticlockwise positive.""" return Rotation2d(self.imu.getYaw()) def get_pose(self) -> Pose2d: """Get the current position of the robot on the field.""" return self.odometry.getPose() @magicbot.feedback def get_heading(self) -> float: """Get the current heading of the robot.""" return self.get_pose().rotation().radians() @magicbot.feedback def get_left_velocity(self) -> float: return self.left_encoder.getVelocity() @magicbot.feedback def get_right_velocity(self) -> float: return self.right_encoder.getVelocity() def reset_odometry(self, pose: Pose2d) -> None: """Resets the odometry to start with the given pose. This is intended to be called at the start of autonomous. """ self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) self.odometry.resetPosition(pose, self._get_heading()) def find_field_angle(self, field_point: Translation2d) -> float: """Return the theoretical angle (in radians) to the target based on odometry""" pose = self.get_pose() rel = field_point - pose.translation() rel_heading = Rotation2d(rel.y, rel.x) + pose.rotation() return rel_heading.radians() def find_power_port_angle(self) -> float: return self.find_field_angle(POWER_PORT_POSITION)
class Path: def __init__(self, kS, kV, trackwidth, trajectory): ''' Creates a controller for following a PathWeaver trajectory. __init__(self, kS: Volts, kV: Volts * Seconds / Meters, trackwidth: Meters, trajectory: wpilib.trajectory.Trajectory) :param kS: The kS gain determined by characterizing the Robot's drivetrain :param kV: The kV gain determined by characterizing the Robot's drivetrain :param trackwidth: The horizontal distance between the left and right wheels of the tank drive. :param trajectory: The trajectory to follow. This can be generated by PathWeaver, or made by hand. ''' self.kS = kS self.kV = kV self.trajectory = trajectory self.odometry = DifferentialDriveOdometry( Rotation2d(radians(0)), self.trajectory.initialPose()) self.ramsete = RamseteController(2, 0.7) self.drive_kinematics = DifferentialDriveKinematics(trackwidth) self.are_wheel_speeds_zero = False self.timer = Timer() def is_done(self): ''' is_done(self) -> bool Returns whether or not the path is done. ''' return self.are_wheel_speeds_zero def reset(self, chassis, gyro): ''' Re-initializes all of the data in the controller as if the path has not yet been executed. This method MUST be called in teleopInit and autonomousInit directly before the controller is used. reset(self, chassis: traits.DriveTrain, gyro: traits.Gyro) :param chassis: An object that implements the DriveTrain trait. This object's encoders will be reset by this function :param gyro: An object that implements the Gyro trait. This object's angle will be reset by this function ''' # Assert the objects implement the proper traits assert chassis.implements(DriveTrain) assert gyro.implements(Gyro) # The encoders and gyro need to be reset so that the Ramsete # controller is fed data that looks new. # By reseting these sensors, we look like weve never run the path at all! chassis.reset_encoders() gyro.reset() self.are_wheel_speeds_zero = False # The odometry object also needs to be re-initialized # so that it forgets the state from the previous run self.odometry = DifferentialDriveOdometry( Rotation2d(radians(0)), self.trajectory.initialPose()) self.timer.start() def follow(self, chassis, gyro): ''' This updates the Path and drives the chassis to follow the path update(self, chassis: traits.DriveTrain, gyro: traits.Gyro) :param chassis: An object that implements the DriveTrain trait. When this function is called, the object's motors will be driven to follow the last set trajectory. :param gyro: An object that implements the gyro trait. ''' # Assert the objects implement the proper traits assert chassis.implements(DriveTrain) assert gyro.implements(Gyro) if self.is_done(): return # Set the chassis to low gear for more precise movements chassis.set_low_gear() # If a trajectory has been set, run it if self.trajectory is not None: # Get the accumulated left and right distance of the chass ld, rd = chassis.get_left_distance(), chassis.get_right_distance() # Ramsete requires the counterclockwise angle of the Robot angle = gyro.get_counterclockwise_degrees() # Get the current position of the robot current_pose = self.odometry.update(Rotation2d(radians(angle)), ld, rd) # Calculate the target position using the trajectory, and get the chassis wheel speeds target_pose = self.trajectory.sample(self.timer.get()) chassis_speed = self.ramsete.calculate(current_pose, target_pose) wheel_speeds = self.drive_kinematics.toWheelSpeeds(chassis_speed) l, r = wheel_speeds.left, wheel_speeds.right if abs(l) == 0 and abs(r) == 0: self.are_wheel_speeds_zero = True # Convert the left and right wheel speeds to volts using the characterized constants, # and then convert those to percent values from -1 to 1 chassis.tank_drive((self.kS + l * self.kV) / 12, (self.kS + r * self.kV) / 12)
class Chassis: # chassis physical constants BUMPER_WIDTH = 3.25 * units.meters_per_inch ROBOT_WIDTH = 30 * units.meters_per_inch + BUMPER_WIDTH ROBOT_LENGTH = 30 * units.meters_per_inch + BUMPER_WIDTH ROBOT_MASS = 100 * units.kilograms_per_pound TRACK_WIDTH = 24 * units.meters_per_inch TRACK_RADIUS = TRACK_WIDTH / 2 WHEEL_DIAMETER = 6 * units.meters_per_inch WHEEL_RADIUS = WHEEL_DIAMETER / 2 WHEEL_CIRCUMFERENCE = 2 * np.pi * WHEEL_RADIUS GEAR_RATIO = (48 / 14) * (50 / 16) # 10.7142861 # conversions RADIANS_PER_METER = (2 * np.pi * GEAR_RATIO) / WHEEL_CIRCUMFERENCE METERS_PER_RADIAN = WHEEL_CIRCUMFERENCE / (2 * np.pi * GEAR_RATIO) # motor config LEFT_INVERTED = True RIGHT_INVERTED = False # motor coefs KS = 0.149 KV = 2.4 KA = 0.234 # velocity pidf gains VL_KP = 0.000363 VL_KI = 0 VL_KD = 0 VL_KF = 0 VR_KP = 0.000363 VR_KI = 0 VR_KD = 0 VR_KF = 0 # joystick MAX_JOYSTICK_OUTPUT = 1 # static objeccts feedforward_l = motorfeedforward.MotorFeedforward(KS, KV, KA) feedforward_r = motorfeedforward.MotorFeedforward(KS, KV, KA) kinematics = DifferentialDriveKinematics(TRACK_WIDTH) # required devices dm_l: lazytalonfx.LazyTalonFX dm_r: lazytalonfx.LazyTalonFX ds_l: lazytalonfx.LazyTalonFX ds_r: lazytalonfx.LazyTalonFX imu: lazypigeonimu.LazyPigeonIMU # constraints MAX_VELOCITY = 3 class _Mode(Enum): Idle = 0 PercentOutput = 1 Velocity = 2 def __init__(self): self.mode = self._Mode.Idle self.odometry = DifferentialDriveOdometry(Rotation2d.fromDegrees(0)) self.desired_output = WheelState() self.desired_velocity = WheelState() self.feedforward = WheelState() self.wheel_left = motorstate.MotorState() self.wheel_right = motorstate.MotorState() self.nt = NetworkTables.getTable(f"/components/chassis") def setup(self): self.dm_l.setInverted(self.LEFT_INVERTED) self.dm_r.setInverted(self.RIGHT_INVERTED) self.dm_l.setRadiansPerUnit(self.RADIANS_PER_METER) self.dm_r.setRadiansPerUnit(self.RADIANS_PER_METER) self.dm_l.setPIDF( 0, self.VL_KP, self.VL_KI, self.VL_KD, self.VL_KF, ) self.dm_r.setPIDF( 0, self.VR_KP, self.VR_KI, self.VR_KD, self.VR_KF, ) def on_enable(self): pass def on_disable(self): self.stop() def stop(self) -> None: self.mode = self._Mode.Idle if wpilib.RobotBase.isSimulation(): self._setSimulationOutput(1, 0) self._setSimulationOutput(3, 0) def setOutput(self, output_l: float, output_r: float) -> None: self.mode = self._Mode.PercentOutput self.desired_output.left = output_l self.desired_output.right = output_r def setWheelVelocity(self, velocity_l: float, velocity_r: float) -> None: self.mode = self._Mode.Velocity self.desired_velocity.left = velocity_l self.desired_velocity.right = velocity_r 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 setTankDrive(self, throttle, rotation): self.mode = self._Mode.PercentOutput self.desired_output.left = throttle + rotation self.desired_output.right = throttle - rotation # self.desired_output.norm(self.MAX_JOYSTICK_OUTPUT) def setForzaDrive(self, throttle, reverse, rotation): if throttle <= 0: throttle = -reverse self.setTankDrive(throttle, rotation) def ntPutLeftRight(self, key, value): self.nt.putNumber(f"{key}_left", value.left) self.nt.putNumber(f"{key}_right", value.right) def updateNetworkTables(self): """Update network table values related to component.""" self.wheel_left.putNT(self.nt, "wheel_left") self.wheel_right.putNT(self.nt, "wheel_right") self.ntPutLeftRight("desired_output", self.desired_output) self.ntPutLeftRight("desired_velocity", self.desired_velocity) self.ntPutLeftRight("feedforward", self.feedforward) def getHeading(self): return self.getPose().rotation().radians() def getPose(self): if wpilib.RobotBase.isSimulation(): x = hal.simulation.SimDeviceSim("Field2D").getDouble("x").get() y = hal.simulation.SimDeviceSim("Field2D").getDouble("y").get() rot = hal.simulation.SimDeviceSim("Field2D").getDouble("rot").get() rot = (units.angle_range(rot * units.radians_per_degree) * units.degrees_per_radian) return Pose2d(x, y, Rotation2d.fromDegrees(rot)) else: return self.odometry.getPose() def _setSimulationOutput(self, id, output): hal.simulation.SimDeviceSim(f"Talon FX[{id}]").getDouble( "Motor Output").set(output) def _getSimulationPosition(self, id): return (hal.simulation.SimDeviceSim( f"Custom Talon FX[{id}]").getDouble("Position").get()) def execute(self): dt = 0.02 self.wheel_left.update(self.dm_l.getPosition(), dt) self.wheel_right.update(self.dm_r.getPosition(), dt) if wpilib.RobotBase.isSimulation(): self.wheel_left.position = self._getSimulationPosition(1) self.wheel_right.position = self._getSimulationPosition(3) self.odometry.update( Rotation2d(self.getHeading()), self.wheel_left.position, self.wheel_right.position, ) if self.mode == self._Mode.Idle: self.dm_l.setOutput(0) self.dm_r.setOutput(0) elif self.mode == self._Mode.PercentOutput: self.dm_l.setOutput(self.desired_output.left) self.dm_r.setOutput(self.desired_output.right) elif self.mode == self._Mode.Velocity: self.feedforward.left = ( self.feedforward_l.calculate(self.desired_velocity.left, dt) / 12) self.feedforward.right = ( self.feedforward_r.calculate(self.desired_velocity.right, dt) / 12) if not wpilib.RobotBase.isSimulation(): self.dm_l.setVelocity( self.desired_velocity.left, self.feedforward.left, ) self.dm_r.setVelocity( self.desired_velocity.right, self.feedforward.right, ) else: self.dm_l.setOutput(self.desired_velocity.left / self.MAX_VELOCITY) self.dm_r.setOutput(self.desired_velocity.right / self.MAX_VELOCITY) self.updateNetworkTables()
def setup(self): self.odometry = DifferentialDriveOdometry( Rotation2d(math.radians(self.getAngle())))