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