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 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 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 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 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):
# 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)