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()