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)