def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTables.getTable("SmartDashboard")
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
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 robotPeriodic(self):
newTime = self.timer.get()
f_l_rate = (self.f_l_encoder.getDistance() -
self.f_l_oldDistance) / (newTime - self.oldTime)
r_l_rate = (self.r_l_encoder.getDistance() -
self.r_l_oldDistance) / (newTime - self.oldTime)
f_r_rate = (self.f_r_encoder.getDistance() -
self.f_r_oldDistance) / (newTime - self.oldTime)
r_r_rate = (self.r_r_encoder.getDistance() -
self.r_r_oldDistance) / (newTime - self.oldTime)
self.oldTime = newTime
self.f_l_oldDistance = self.f_l_encoder.getDistance()
self.r_l_oldDistance = self.r_l_encoder.getDistance()
self.f_r_oldDistance = self.f_r_encoder.getDistance()
self.r_r_oldDistance = self.r_r_encoder.getDistance()
# Odometry Update
# First, get the wheel speeds...
self.wheelSpeeds = MecanumDriveWheelSpeeds(
f_l_rate,
r_l_rate,
f_r_rate,
r_r_rate,
)
# Next, we need to grab the Gyro angle and send it into the odometry. It must be inverted because gyros v Wpilib are backwards
gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle())
# Finally, we can update the pose...
self.m_pose = self.MecanumDriveOdometry.update(gyroAngle,
self.wheelSpeeds)
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 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 test_swerve4_odometry(s4: SwerveDrive4Kinematics):
odometry = SwerveDrive4Odometry(s4, Rotation2d(0))
odometry.resetPosition(Pose2d(), Rotation2d.fromDegrees(90))
state = SwerveModuleState(5)
odometry.updateWithTime(
0,
Rotation2d.fromDegrees(90),
SwerveModuleState(),
SwerveModuleState(),
SwerveModuleState(),
SwerveModuleState(),
)
pose = odometry.updateWithTime(0.1, Rotation2d.fromDegrees(90), state,
state, state, state)
assert pose.translation().x == pytest.approx(0.5)
assert pose.translation().y == pytest.approx(0.0)
assert pose.rotation().degrees() == pytest.approx(0)
def getPose(self) -> Optional[Pose2d]:
if not self.targetExists():
return None
data = self.pose_data
# Turn limelight pose information into a wpilib Pose2d
rot = Rotation2d.fromDegrees((data[4] + 180) % 360)
x = 15 * math.cos(rot.degrees()) + (-data[2])
y = 15 * math.cos(90 - rot.degrees()) + data[0]
# Change unit from inches to meters
return Pose2d(x / 39.37, y / 39.37, rot)
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_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 robotPeriodic(self):
# Odometry Update
# First, get the wheel speeds...
self.wheelSpeeds = MecanumDriveWheelSpeeds(
self.f_l_encoder.getRate(),
self.r_l_encoder.getRate(),
self.f_r_encoder.getRate(),
self.r_r_encoder.getRate(),
)
# Next, we need to grab the Gyro angle and send it into the odometry. It must be inverted because gyros v Wpilib are backwards
gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle())
# Finally, we can update the pose...
self.m_pose = self.MecanumDriveOdometry.update(gyroAngle,
self.wheelSpeeds)
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 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)
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) -> 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)))
class Waypoints:
MIN_FIELD_X = 0
MAX_FIELD_X = 54 * units.meters_per_foot
MIN_FIELD_Y = 0
MAX_FIELD_Y = 27 * units.meters_per_foot
INITATION_LINE_X = 10 * units.meters_per_foot
OFFSET = 180
START_LAWFUL = Pose2d(INITATION_LINE_X, 7.3, Rotation2d.fromDegrees(180))
TRENCH_RUN_LAWFUL = Pose2d(8.3, 7.3, Rotation2d.fromDegrees(180))
START_STEAL = Pose2d(INITATION_LINE_X, 0.7, Rotation2d.fromDegrees(180))
TRENCH_RUN_STEAL = Pose2d(6.2, 0.7, Rotation2d.fromDegrees(180))
START_CENTER = Pose2d(INITATION_LINE_X, 5.8, Rotation2d.fromDegrees(180))
RENDEZVOUS_TWO_BALLS = Pose2d(6.3, 5.5, Rotation2d.fromDegrees(120))
IDEAL_SHOOT = Pose2d(INITATION_LINE_X + 2 * units.meters_per_foot, 5.8,
Rotation2d.fromDegrees(180))
def setup(self):
self.odometry = DifferentialDriveOdometry(
Rotation2d(math.radians(self.getAngle())))
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()
class StartingPosition(Enum):
# LEFT is one foot off the LEFT wall relative to the Power port (and facing the power port).
LEFT = Pose2d(3.2, -0.6468, Rotation2d.fromDegrees(180))
CENTER = Pose2d(3.2, 0, Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
RIGHT = Pose2d(3.2, -3.25,
Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
@dataclass
class TrajectoryData:
"""Hold metadata for a wpilib trajectory"""
start: Pose2d
interior_waypoints: List[Translation2d]
end: Pose2d
config: TrajectoryConfig = TrajectoryConfig(MAX_GENERATION_VELOCITY,
MAX_GENERATION_ACCELERATION)
constraints: Tuple[TrajectoryConstraint, ...] = DEFAULT_CONSTRAINTS
kinematics: DifferentialDriveKinematics = KINEMATICS
field_relative: bool = True
reverse: bool = False
# The POWER PORT's Pose relative to the top left coordinate of the field as provided by Path weaver.
POWER_PORT = Pose2d(0.2, -2.437, Rotation2d())
# Starting positions are relative to the field's top left coordinate facing down the field
class StartingPosition(Enum):
# LEFT is one foot off the LEFT wall relative to the Power port (and facing the power port).
LEFT = Pose2d(3.2, -0.6468, Rotation2d.fromDegrees(180))
CENTER = Pose2d(3.2, 0, Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
RIGHT = Pose2d(3.2, -3.25,
Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
# ALL trajectory points should be relative to the field's top left coordinate down the field
# Trajectories that aren't field relative are generated with respect to ALL Starting Positions
# E.x. "charge-LEFT", "charge-CENTER", "charge-RIGHT"
# We do this to allow for trajectory chaining, which requires field-relativity
def getAveragedPose(self) -> Pose2d:
return Pose2d(self.avg_x, self.avg_y,
Rotation2d.fromDegrees(self.avg_rot))
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 _get_heading(self) -> Rotation2d:
"""Get the current heading of the robot from the IMU, anticlockwise positive."""
return Rotation2d(self.imu.getYaw())
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)
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
self.sd = NetworkTables.getTable("SmartDashboard")
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
#Create the DriveTrain
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
#Create The Encoders
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_l_encoder.setSimDevice(0)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_l_encoder.setSimDevice(1)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_r_encoder.setSimDevice(2)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_r_encoder.setSimDevice(3)
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
# Now that we have created the ability to see wheel speeds, chassis speeds and our position
# Let us start to use feedforward to try to lock our robot into a specific speed.
self.feedForward = SimpleMotorFeedforwardMeters(kS=0.194,
kV=.5,
kA=0.457)