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