class DriveTrainSubsystem(Subsystem):
def __init__(self):
self.robot = SpartanRobot.getRobotObject(self)
self.setDefaultCommand(ArcadeDriveCommand())
#Power output to motors in range of -1 to 1
self.leftPower = 0
self.rightPower = 0
self.leftEncoder = Encoder(RobotMap.leftDriveEncoder1, RobotMap.leftDriveEncoder2, False,
Encoder.EncodingType.k4X)
self.rightEncoder = Encoder(RobotMap.rightDriveEncoder1, RobotMap.rightDriveEncoder2, True,
Encoder.EncodingType.k4X)
self.gyro = ADXRS450_Gyro()
self.rightDriveMotor1 = VictorSP(RobotMap.rightDriveMotor1)
self.rightDriveMotor2 = VictorSP(RobotMap.rightDriveMotor2)
# self.rightDriveMotor3 = VictorSP(rightDriveMotor3)
self.leftDriveMotor1 = VictorSP(RobotMap.leftDriveMotor1)
self.leftDriveMotor2 = VictorSP(RobotMap.leftDriveMotor2)
# self.leftDriveMotor3 = VictorSP(leftDriveMotor3)
self.leftEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks)
self.rightEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks)
self.leftEncoder.setMaxPeriod(5)
self.rightEncoder.setMaxPeriod(5)
self.leftEncoder.setMinRate(0)
self.rightEncoder.setMinRate(0)
self.leftEncoder.setSamplesToAverage(1)
self.rightEncoder.setSamplesToAverage(1)
self.gyro.calibrate()
def setLeftDrivePower(self, power):
self.leftPower = power
def setRightDrivePower(self, power):
self.rightPower = power
def updateMotorOutputs(self):
self.leftDriveMotor1 = -self.leftPower
self.leftDriveMotor2 = -self.leftPower
# self.leftDriveMotor3 = -self.leftPower
self.rightDriveMotor1 = self.rightPower
self.rightDriveMotor2 = self.rightPower
# self.rightDriveMotor3 = self.rightPower
def putEncoderValues(self):
SmartDashboard.putNumber("Left Encoder Raw", self.leftEncoder.getRaw())
SmartDashboard.putNumber("Right Encoder Raw", self.rightEncoder.getRaw())
SmartDashboard.putNumber("Left Encoder Dist Per Pulse", self.leftEncoder.getDistancePerPulse())
SmartDashboard.putNumber("Right Encoder Dist Per Pulse", self.rightEncoder.getDistancePerPulse())
def getLeftDistance(self):
return self.leftEncoder.getDistance()
def getRightDistance(self):
return self.rightEncoder.getDistance()
def resetGyro(self):
self.gyro.reset()
def resetEncoders(self):
self.leftEncoder.reset()
self.rightEncoder.reset()
def accelerateDrive(self):
if self.robot.oi.xBoxController.getRawAxis(1) > 0:
return math.pow(self.robot.oi.xBoxController.getRawAxis(1), 2)
else:
return -math.pow(self.robot.oi.xBoxController.getRawAxis(1), 2)
class DriveBase:
left_motor_one = CANTalon
left_motor_two = CANTalon
right_motor_one = CANTalon
right_motor_two = CANTalon
left_encoder = Encoder
right_encoder = Encoder
navx = AHRS
def __init__(self):
super().__init__()
"""
Motor objects init
Reason for recall is because MagicRobot is looking for the CANTalon
Object instance before init
"""
self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor)
self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor)
self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor)
self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor)
self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.navx = AHRS(SPI.Port.kMXP)
self.left_motor_one.enableBrakeMode(True)
self.left_motor_two.enableBrakeMode(True)
self.right_motor_one.enableBrakeMode(True)
self.right_motor_two.enableBrakeMode(True)
self.base = RobotDrive(self.left_motor_one, self.left_motor_two,
self.right_motor_one, self.right_motor_two)
self.dpp = sensor_map.wheel_diameter * math.pi / 360
self.left_encoder.setDistancePerPulse(self.dpp)
self.right_encoder.setDistancePerPulse(self.dpp)
self.left_encoder.setSamplesToAverage(sensor_map.samples_average)
self.right_encoder.setSamplesToAverage(sensor_map.samples_average)
self.left_encoder.setMinRate(sensor_map.min_enc_rate)
self.right_encoder.setMinRate(sensor_map.min_enc_rate)
self.auto_pid_out = AutoPIDOut()
self.pid_d_controller = PIDController(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F,
self.navx, self.auto_pid_out, 0.005)
self.type_flag = ("DRIVE", "TURN")
self.current_flag = self.type_flag[0]
self.auto_pid_out.drive_base = self
self.auto_pid_out.current_flag = self.current_flag
def drive(self, left_power, right_power):
self.base.tankDrive(left_power, right_power)
def execute(self):
if int(self.base.getNumMotors()) < 3:
self.base.drive(0, 0)
def get_drive_distance(self):
return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance())
def rest_base(self):
self.left_encoder.reset()
self.right_encoder.reset()
def pid_drive(self, speed, distance, to_angle=None):
self.navx.isCalibrating()
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F)
self.pid_d_controller.setOutputRange(speed - distance, speed + distance)
if to_angle is None:
set_angle = self.navx.getYaw()
else:
set_angle = to_angle
self.pid_d_controller.setSetpoint(float(set_angle))
self.drive(speed + 0.03, speed)
self.pid_d_controller.enable()
self.current_flag = self.type_flag[0]
self.auto_pid_out.current_flag = self.current_flag
def pid_turn(self, angle):
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.turn_P,
sensor_map.turn_I,
sensor_map.turn_D,
sensor_map.turn_F)
self.pid_d_controller.setOutputRange(sensor_map.output_range_min,
sensor_map.output_range_max)
self.pid_d_controller.setSetpoint(float(angle))
self.pid_d_controller.enable()
self.current_flag = self.type_flag[1]
self.auto_pid_out.current_flag = self.current_flag
def stop_pid(self):
self.pid_d_controller.disable()
self.pid_d_controller.reset()
