def __init__(self,
driveCanTalonId,
steerCanTalonId,
absoluteEncoder=True,
reverseDrive=False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(
CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
self.launchMin = self.k_launchMin
self.launchMax = self.k_launchMax
self.launchRange = self.launchMax - self.launchMin
self.controlMode = None
self.visionState = robot.visionState
self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId)
self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeLeftMotor.reverseSensor(True)
self.intakeLeftMotor.setSafetyEnabled(False)
self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId)
self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeRightMotor.setSafetyEnabled(False)
self.aimMotor = CANTalon(robot.map.k_IlAimMotorId)
# LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor)
if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot):
self.aimMotor.setSafetyEnabled(False)
self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot)
self.aimMotor.enableLimitSwitch(True, True)
self.aimMotor.enableBrakeMode(True)
self.aimMotor.setAllowableClosedLoopErr(5)
self.aimMotor.configPeakOutpuVoltage(12.0, -12.0)
self.aimMotor.setVoltageRampRate(150)
# TODO: setPID if needed
self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort)
self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort)
self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
class DriveMotors():
def __init__(self, robot):
self.robot = robot
self.motor_a = CANTalon(RobotMap.drive_motors_motor_a_id)
self.motor_b = CANTalon(RobotMap.drive_motors_motor_b_id)
def drive(self, speed):
self.motor_a.set(speed)
self.motor_b.set(speed)
class DriveMotors():
def __init__(self, robot):
self.robot = robot
self.motor_a = CANTalon(RobotMap.drive_motors_motor_a_id)
self.motor_b = CANTalon(RobotMap.drive_motors_motor_b_id)
def drive(self, speed):
self.motor_a.set(speed)
self.motor_b.set(speed)
def __init__(self,
drive,
steer,
absolute=True,
reverse_drive=False,
reverse_steer=False,
zero_reading=0,
drive_encoder=False,
reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(
CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian * 2.0 * math.pi / 4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80 * 6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def __init__(self, robot):
super().__init__()
self.robot = robot
self.armMotor = CANTalon(4)
self.wheelMotor = CANTalon(5)
self.frontSwitch = DigitalInput(8)
self.backSwitch = DigitalInput(9)
self.comp = Compressor()
self.comp.enabled()
self.armMotor.enableBrakeMode(True)
self.wheelMotor.enableBrakeMode(True)
self.potentiometer = AnalogPotentiometer(3, 270, -193)
#self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02)
self.position = 0
def __init__(self, drive, steer,
absolute=True, reverse_drive=False,
reverse_steer=False, zero_reading=0,
drive_encoder=False, reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian*2.0*math.pi/4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80*6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder = True, reverseDrive = False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def __init__(self, robot, name = None):
super().__init__(name = name)
self.robot = robot
self.leftMotor = CANTalon(robot.map.k_PcLeftMotorId)
self.leftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.leftMotor.enableLimitSwitch(True, True);
self.leftMotor.enableBrakeMode(True);
self.leftMotor.configPeakOutputVoltage(+12.0, -12.0);
self.leftMotor.setSafetyEnabled(False);
self.rightMotor = CANTalon(robot.map.k_PcRightMotorId)
self.rightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.rightMotor.enableLimitSwitch(True, True);
self.rightMotor.enableBrakeMode(True);
self.rightMotor.configPeakOutputVoltage(+12.0, -12.0);
self.rightMotor.setSafetyEnabled(False);
self.barMotor = CANTalon(robot.map.k_PcBarMotorId)
self.barMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.barMotor.enableBrakeMode(True);
self.barMotor.setSafetyEnabled(False);
def __init__(self, robot):
super().__init__()
self.robot = robot
if RobotMap.motor_controllers == "talonsrx":
self._motors = [
CANTalon(RobotMap.motor_a_talon_id),
CANTalon(RobotMap.motor_b_talon_id),
CANTalon(RobotMap.motor_c_talon_id),
CANTalon(RobotMap.motor_d_talon_id)
]
for motor in self._motors:
motor.changeControlMode(RobotMap.drive_motor_mode)
elif RobotMap.motor_controllers == "victor":
self._motors = [
Victor(RobotMap.motor_a_pwm_id),
Victor(RobotMap.motor_b_pwm_id),
Victor(RobotMap.motor_c_pwm_id),
Victor(RobotMap.motor_d_pwm_id)
]
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
from grt.sensors.attack_joystick import Attack3Joystick from grt.sensors.xbox_joystick import XboxJoystick #from grt.sensors.gyro import Gyro from grt.core import SensorPoller from grt.mechanism.drivetrain import DriveTrain from grt.mechanism.drivecontroller import ArcadeDriveController from grt.mechanism.motorset import Motorset from grt.sensors.ticker import Ticker from grt.sensors.encoder import Encoder from grt.sensors.talon import Talon from grt.mechanism.mechcontroller import MechController #DT Talons and Objects dt_right = CANTalon(1) dt_r2 = CANTalon(2) dt_r3 = CANTalon(3) dt_r4 = CANTalon(4) dt_left = CANTalon(7) dt_l2 = CANTalon(8) dt_l3 = CANTalon(9) dt_l4 = CANTalon(10) dt_r2.changeControlMode(CANTalon.ControlMode.Follower) dt_r3.changeControlMode(CANTalon.ControlMode.Follower) dt_r4.changeControlMode(CANTalon.ControlMode.Follower) dt_l2.changeControlMode(CANTalon.ControlMode.Follower) dt_l3.changeControlMode(CANTalon.ControlMode.Follower) dt_l4.changeControlMode(CANTalon.ControlMode.Follower)
class Portcullis(Subsystem):
def __init__(self, robot, name = None):
super().__init__(name = name)
self.robot = robot
self.leftMotor = CANTalon(robot.map.k_PcLeftMotorId)
self.leftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.leftMotor.enableLimitSwitch(True, True);
self.leftMotor.enableBrakeMode(True);
self.leftMotor.configPeakOutputVoltage(+12.0, -12.0);
self.leftMotor.setSafetyEnabled(False);
self.rightMotor = CANTalon(robot.map.k_PcRightMotorId)
self.rightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.rightMotor.enableLimitSwitch(True, True);
self.rightMotor.enableBrakeMode(True);
self.rightMotor.configPeakOutputVoltage(+12.0, -12.0);
self.rightMotor.setSafetyEnabled(False);
self.barMotor = CANTalon(robot.map.k_PcBarMotorId)
self.barMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.barMotor.enableBrakeMode(True);
self.barMotor.setSafetyEnabled(False);
# self.leftMotor.setForwardSoftLimit(PORTCULLIS_TOP);
# self.leftMotor.setReverseSoftLimit(PORTCULLIS_BOT);
# self.leftMotor.enableForwardSoftLimit(true);
# self.leftMotor.enableReverseSoftLimit(true);
def initDefaultCommand(self):
# currently we don't have a default command
pass
def updateDashboard(self):
SmartDashboard.putBoolean("Portcullis Upper Right Limit Switch",
self.rightAtTop())
SmartDashboard.putBoolean("Portcullis Upper Left Limit Switch",
self.leftAtTop())
SmartDashboard.putBoolean("Portcullis Lower Right Limit Switch",
self.rightAtBottom())
SmartDashboard.putBoolean("Portcullis Lower Left Limit Switch",
self.leftAtBottom())
def leftAtTop(self):
return self.leftMotor.isFwdLimitSwitchClosed()
def leftAtBottom(self):
return self.leftMotor.isRevLimitSwitchClosed()
def rightAtTop(self):
return self.rightMotor.isFwdLimitSwitchClosed();
def rightAtBottom(self):
return self.rightMotor.isRevLimitSwitchClosed();
def moveRightUp(self):
self.rightMotor.set(s_pctPowerUp)
if self.RightAtTop():
self.StopRight()
self.robot.log("Right Portcullis reached top")
def moveRightDown(self):
self.rightMotor.set(s_pctPowerDown)
if self.RightAtBottom():
self.StopRight()
self.robot.log("Right Portcullis reached bottom")
def stopRight(self):
self.rightMotor.set(0)
def moveLeftUp(self):
self.leftMotor.set(s_pctPowerUp)
if self.LeftAtTop():
self.StopLeft()
self.robot.log("Left Portcullis reached top")
def moveLeftDown(self):
self.leftMotor.set(s_pctPowerDown)
if self.LeftAtBottom():
self.StopLeft()
self.robot.log("Left Portcullis reached bottom")
def stopLeft(self):
self.leftMotor.set(0)
def barIn(self):
if self.LeftAtBottom() and self.RightAtBottom():
self.barMotor.set(s_barIn);
else:
self.StopBar()
def barOut(self):
self.barMotor.set(s_barOut)
def stopBar(self):
self.barMotor.set(0)
class SwerveModule():
def __init__(self,
driveCanTalonId,
steerCanTalonId,
absoluteEncoder=True,
reverseDrive=False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(
CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
# Always in radians. Right hand rule applies - Z is up!
# Rescale values to the range [-pi, pi)
def steer(self, direction, speed=0):
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self._direction)
delta = min_angular_displacement(current_heading, direction)
self._direction += delta
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self._direction) - direction) < math.pi / 6.0:
self._drive.set(speed)
self._speed = speed
else:
self._drive.set(-speed)
self._speed = -speed
if self.absoluteEncoder:
self._steer.set(self._direction / TAU *
RobotMap.module_rotation_volts_per_revolution)
else:
self._steer.set(self._direction / TAU *
RobotMap.module_rotation_counts_per_revolution)
def getSpeed(self):
return self._speed
def getDirection(self):
return self._direction
class SwerveModule():
def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder = True, reverseDrive = False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
# Always in radians. Right hand rule applies - Z is up!
# Rescale values to the range [-pi, pi)
def steer(self, direction, speed = 0):
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self._direction)
delta = min_angular_displacement(current_heading, direction)
self._direction += delta
if self.reverse_drive:
speed=-speed
if abs(constrain_angle(self._direction)-direction)<math.pi/6.0:
self._drive.set(speed)
self._speed = speed
else:
self._drive.set(-speed)
self._speed = -speed
if self.absoluteEncoder:
self._steer.set(self._direction/TAU*RobotMap.module_rotation_volts_per_revolution)
else:
self._steer.set(self._direction/TAU*RobotMap.module_rotation_counts_per_revolution)
def getSpeed(self):
return self._speed
def getDirection(self):
return self._direction
class arm(Component):
#set up variables
def __init__(self, robot):
super().__init__()
self.robot = robot
self.armMotor = CANTalon(4)
self.wheelMotor = CANTalon(5)
self.frontSwitch = DigitalInput(8)
self.backSwitch = DigitalInput(9)
self.comp = Compressor()
self.comp.enabled()
self.armMotor.enableBrakeMode(True)
self.wheelMotor.enableBrakeMode(True)
self.potentiometer = AnalogPotentiometer(3, 270, -193)
#self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02)
self.position = 0
def armAuto(self, upValue, downValue, target, rate=0.3):
'''
if self.getPOT() <= target:
self.armMotor.set(0)
'''
if upValue == 1:
self.armMotor.set(rate * -1)
elif downValue == 1:
self.armMotor.set(rate)
else:
self.armMotor.set(0)
def armUpDown(self, left, right, controllerA, rate=0.3):
rate2 = rate*1.75
if(self.backSwitch.get() == False or self.frontSwitch.get() == False): #Checking limit switches
self.armMotor.set(0)
if(left >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(rate2)
# else:
self.armMotor.set(rate)
elif(right >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(-rate2)
# else:
self.armMotor.set(rate * -1)
elif(left < 0.75 and right < 0.75):
self.armMotor.set(0)
def wheelSpin(self, speed):
currentSpeed = 0
if (speed > 0.75):
currentSpeed = -1
elif(speed < -0.75):
currentSpeed = 1
self.wheelMotor.set(currentSpeed)
def getPOT(self):
return (self.potentiometer.get()*-1)
def getBackSwitch(self):
return self.backSwitch.get()
def getFrontSwitch(self):
return self.frontSwitch.get()
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()
class GRTTalon:
def __init__(self, channel):
self.t = CANTalon(channel)
self.channel = channel
def set(self, power):
self.t.set(power)
def get(self):
return self.t.get()
print(self.t.Get())
def getDeviceID(self):
return self.t.getDeviceID()
def changeControlMode(self, mode):
self.t.changeControlMode(mode)
def setP(self, value):
self.t.setP(value)
def setI(self, value):
self.t.setI(value)
def setD(self, value):
self.t.setD(value)
def poll(self):
# self.busVoltage = t.getBusVoltage()
# self.closeLoopRampRate = t.getCloseLoopRampRate()
self.controlMode = t.getControlMode()
self.deviceID = t.getDeviceID()
self.encPosition = t.getEncPosition()
self.encVelocity = t.getEncVelocity()
self.outputCurrent = t.getOutputCurrent()
self.outputVoltage = t.getOutputVoltage()
self.closeLoopError = t.getCloseLoopError()
def __init__(self, robot):
self.robot = robot
self.motor_a = CANTalon(RobotMap.drive_motors_motor_a_id)
self.motor_b = CANTalon(RobotMap.drive_motors_motor_b_id)
def __init__(self, robot):
self.robot = robot
self.motor_a = CANTalon(RobotMap.drive_motors_motor_a_id)
self.motor_b = CANTalon(RobotMap.drive_motors_motor_b_id)
class Shooter:
left_fly = CANTalon
right_fly = CANTalon
intake_main = CANTalon
intake_mecanum = Talon
ball_limit = DigitalInput
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def warm_up(self):
self.set_rpm(2500) # Warm up flywheels to get ready to shoot
def low_goal(self):
self.set_rpm(500)
def set_rpm(self, rpm_l_set, rpm_r_set=None):
if rpm_r_set is None:
rpm_r_set = rpm_l_set
self.left_fly.set(rpm_l_set)
self.right_fly.set(rpm_r_set)
def get_rpms(self):
return self.left_fly.get(), self.right_fly.get()
def set_fly_off(self):
self.set_rpm(0)
def set_intake(self, power):
self.intake_mecanum.set(-power)
self.intake_main.set(power)
def get_intake(self):
return self.intake_main.get()
def set_intake_off(self):
self.set_intake(0)
def get_ball_limit(self):
return not bool(self.ball_limit.get())
def execute(self):
if int(self.left_fly.getOutputCurrent()) > 20 \
or int(self.left_fly.getOutputCurrent()) > 20:
self.set_rpm(0, 0)
def __init__(self):
self.chopper_motor = CANTalon(motor_map.chopper_motor)
self.chopper_motor.setSafetyEnabled(False)
self.chopper_motor.enableLimitSwitch(True, True)
class SwerveModule():
def __init__(self, drive, steer,
absolute=True, reverse_drive=False,
reverse_steer=False, zero_reading=0,
drive_encoder=False, reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian*2.0*math.pi/4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80*6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def changeDriveControlMode(self, control_mode):
if self._drive.getControlMode is not control_mode:
if control_mode == CANTalon.ControlMode.Speed:
self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed)
elif control_mode == CANTalon.ControlMode.Position:
self._drive.setPID(0.1, 0.0, 0.0, 0.0)
self._drive.changeControlMode(control_mode)
@property
def direction(self):
# Read the current direction from the controller setpoint
setpoint = self._steer.getSetpoint()
return float(setpoint - self._offset) / self.counts_per_radian
@property
def speed(self):
# Read the current speed from the controller setpoint
setpoint = self._drive.getSetpoint()
return float(setpoint)
@property
def distance(self):
# Read the current position from the encoder and remove the offset
return self._drive.getEncPosition() - self._distance_offset
def zero_distance(self):
self._distance_offset = self._drive.getEncPosition()
def steer(self, direction, speed=None):
if self.drive_encoder:
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
else:
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
if speed is None:
# Force the modules to the direction specified - don't
# go to the closest one and reverse.
delta = constrain_angle(direction - self.direction) # rescale to +/-pi
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
self._drive.set(0.0)
return
if abs(speed) > 0.05:
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self.direction)
delta = min_angular_displacement(current_heading, direction)
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0:
self._drive.set(speed*self.drive_max_speed)
else:
self._drive.set(-speed*self.drive_max_speed)
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
else:
self._drive.set(0.0)
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
class driveTrain(Component) :
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
# Adding components to the LiveWindow (testing)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor)
'''
def log(self):
#The log method puts interesting information to the SmartDashboard. (like velocity information)
'''
#no longer implemented because of change of hardware
wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity())
'''
wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition())
wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition())
wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition())
wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition())
'''
wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57))
'''
# drive forward function
def drive_forward(self, speed) :
self.drive.tankDrive(speed, speed, True)
# manual drive function for Tank Drive
def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT):
#self.lfmotor.setCloseLoopRampRate(1)
#self.lbmotor.setCloseLoopRampRate(1)
#self.rfmotor.setCloseLoopRampRate(1)
#self.rbmotor.setCloseLoopRampRate(1)
if (leftB == True): #Straight Button
rightSpeed = leftSpeed
if (rightB == True): #Slow Button
#leftSpeed = leftSpeed/1.75
#rightSpeed = rightSpeed/1.75
if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning
leftSpeed = leftSpeed/1.75
rightSpeed = rightSpeed/1.75
# Fast button
if(rightT == True):
#self.lfmotor.setCloseLoopRampRate(24)
#self.lbmotor.setCloseLoopRampRate(24)
#self.rfmotor.setCloseLoopRampRate(24)
#self.rbmotor.setCloseLoopRampRate(24)
leftSpeed = leftSpeed*(1.75)
rightSpeed = rightSpeed*(1.75)
if(leftT == True):
leftSpeed = 0.1
rightSpeed = 0.1
# Creating margin for error when using the joysticks, as they're quite sensitive
if abs(rightSpeed) < 0.04 :
rightSpeed = 0
if abs(leftSpeed) < 0.04 :
leftSpeed = 0
if self.CONTROL_TYPE:
self.pidRightFront.setSetpoint(rightSpeed)
self.pidRightBack.setSetpoint(rightSpeed)
self.pidLeftFront.setSetpoint(leftSpeed)
self.pidLeftBack.setSetpoint(leftSpeed)
else:
self.lfmotor.set(leftSpeed*512)
self.rfmotor.set(rightSpeed*512)
self.lbmotor.set(leftSpeed*512)
self.rbmotor.set(rightSpeed*512)
#autononmous tank drive (to remove a need for a slow, striaght, or fast button)
def autonTankDrive(self, leftSpeed, rightSpeed):
self.log()
#self.drive.tankDrive(leftSpeed, rightSpeed, True)
self.rfmotor.set(rightSpeed)
self.rbmotor.set(rightSpeed*(-1))
self.lfmotor.set(leftSpeed)
self.lbmotor.set(leftSpeed*(-1))
# stop function
def drive_stop(self) :
self.drive.tankDrive(0,0)
# fucntion to reset the PID's and encoder values
def reset(self):
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
if self.CONTROL_TYPE:
self.LEFTFRONTCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE= 0
self.RIGHTBACKCUMULATIVE = 0
self.pidLeftBack.setSetpoint(0)
self.pidLeftFront.setSetpoint(0)
self.pidRightBack.setSetpoint(0)
self.pidRightFront.setSetpoint(0)
# def getDistance(self)
# return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE)))
def turn_angle(self, degrees):
desired_inches = self.INCHES_PER_DEGREE * degrees
if degrees < 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(0.4, -0.4)
elif degrees > 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(-0.4, 0.4)
# Enable PID Controllers
def enablePIDs(self):
'''
#No longer required because we swapped from analog encoders to magnetic encoders
self.pidLeftFront.enable()
self.pidLeftBack.enable()
self.pidRightFront.enable()
self.pidRightBack.enable()
'''
# Disable PID Controllers
def disablePIDs(self):
'''
#see explaination above
self.pidLeftFront.disable()
self.pidLeftBack.disable()
self.pidRightFront.disable()
self.pidRightBack.disable()
'''
def getAutonDistance(self):
return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4
#detirmines how many ticks the encoder has processed
def getMotorDistance(self, motor, cumulativeDistance):
currentRollovers = 0 #number of times the encoder has gone from 1023 to 0
previousValue = cumulativeDistance #variable for comparison
currentValue = motor.getEncPosition() #variable for comparison
if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0
currentRollovers += 1 #notes the rollover
return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks
#converts ticks from getMotorDistance into inches
def convertEncoderRaw(self, selectedEncoderValue):
return selectedEncoderValue * self.INCHES_PER_REV
class SwerveModule():
def __init__(self, drive, steer,
absolute=True, reverse_drive=False,
reverse_steer=False, zero_reading=0,
drive_encoder=False, reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian*2.0*math.pi/4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80*6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def changeDriveControlMode(self, control_mode):
if self._drive.getControlMode is not control_mode:
if control_mode == CANTalon.ControlMode.Speed:
self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed)
elif control_mode == CANTalon.ControlMode.Position:
self._drive.setPID(0.1, 0.0, 0.0, 0.0)
self._drive.changeControlMode(control_mode)
@property
def direction(self):
# Read the current direction from the controller setpoint
setpoint = self._steer.getSetpoint()
return float(setpoint - self._offset) / self.counts_per_radian
@property
def speed(self):
# Read the current speed from the controller setpoint
setpoint = self._drive.getSetpoint()
return float(setpoint)
@property
def distance(self):
# Read the current position from the encoder and remove the offset
return self._drive.getEncPosition() - self._distance_offset
def zero_distance(self):
self._distance_offset = self._drive.getEncPosition()
def steer(self, direction, speed=None):
if self.drive_encoder:
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
else:
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
if speed is None:
# Force the modules to the direction specified - don't
# go to the closest one and reverse.
delta = constrain_angle(direction - self.direction) # rescale to +/-pi
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
self._drive.set(0.0)
return
if abs(speed) > 0.05:
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self.direction)
delta = min_angular_displacement(current_heading, direction)
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0:
self._drive.set(speed*self.drive_max_speed)
else:
self._drive.set(-speed*self.drive_max_speed)
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
else:
self._drive.set(0.0)
from grt.mechanism.drivecontroller import ArcadeDriveController
from grt.sensors.encoder import Encoder
from grt.mechanism.mechcontroller import MechController
from grt.sensors.navx import NavX
from grt.macro.straight_macro import StraightMacro
from grt.mechanism.pickup import Pickup
from grt.mechanism.manual_shooter import ManualShooter
from queue import Queue
#Compressor initialization
c = Compressor()
c.start()
#Manual pickup Talons and Objects
pickup_achange_motor1 = CANTalon(45)
pickup_achange_motor2 = CANTalon(46)
pickup_achange_motor1.changeControlMode(CANTalon.ControlMode.Follower)
pickup_achange_motor1.set(47)
pickup_achange_motor1.reverseOutput(True)
pickup_roller_motor = CANTalon(8)
pickup = Pickup(pickup_achange_motor1, pickup_achange_motor2,
pickup_roller_motor)
#Manual shooter Talons and Objects
flywheel_motor = CANTalon(44)
shooter_act = Solenoid(1)
turntable_motor = CANTalon(12)
class DriveTrain(Subsystem):
"""DriveTrain: is the subsystem responsible for motors and
devices associated with driving subystem.
As a subsystem, we represent the single point of contact
for all drivetrain-related controls. Specifically, commands
that manipulate the drivetrain should 'require' the singleton
instance (via require(robot.driveTrain)). Unless overridden,
our default command, JoystickDriver, is the means by which
driving occurs.
"""
k_minThrottleScale = 0.5
k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally
k_maxDriveSpeed = 150.0 # ~20 ft/sec
k_maxTurnSpeed = 40.0 # ~3-4 ft/sec
k_fastTurn = -1
k_mediumTurn = -.72
k_slowTurn = -.55
k_quadTicksPerWheelRev = 9830
k_wheelDiameterInInches = 14.0
k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi
k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches
k_turnKp = .1
k_turnKi = 0
k_turnKd = .3
k_turnKf = .001
k_ControlModeSpeed = 0
k_ControlModeVBus = 1
k_ControlModeDisabled = 2
class _turnHelper(PIDOutput):
"""a private helper class for PIDController-based
imu-guided turning.
"""
def __init__(self, driveTrain):
super().__init__()
self.driveTrain = driveTrain
def pidWrite(self, output):
self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed)
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
def initForCommand(self, controlMode):
self.leftMasterMotor.setEncPosition(0) # async call
self.rightMasterMotor.setEncPosition(0)
self.robotDrive.stopMotor()
self.robotDrive.setMaxOutput(self.maxSpeed)
if controlMode == self.k_ControlModeSpeed:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
elif controlMode == self.k_ControlModeVBus:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
elif controlMode == self.k_ControlModeDisabled:
# unverified codepath
self.leftMasterMotor.disableControl()
self.rightMasterMotor.disableControl()
else:
self.robot.error("Unexpected control mode")
self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" %
(self.leftMasterMotor.getControlMode(),
self.rightMasterMotor.getControlMode()))
def initDefaultCommand(self):
# control modes are integers values:
# 0 percentvbux
# 1 position
# 2 velocity
self.setDefaultCommand(JoystickDriver(self.robot))
self.robotDrive.stopMotor();
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
def stop(self):
self.robotDrive.stopMotor()
def joystickDrive(self, jsY, jsX, throttle):
""" joystickDrive - called by JoystickDriver command. Inputs
are always on the range [-1, 1]... These values can be scaled
for a better "feel", but should always be in a subset of this
range.
"""
if self.robot.isAutonomous or \
(math.fabs(jx) < 0.075 and math.fabs(jy) < .075):
# joystick dead zone or auto (where we've been scheduled via
# default command machinery)
self.robotDrive.stopMotor()
else:
st = self.scaleThrottle(throttle)
self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st)
def drive(self, outputmag, curve):
""" drive - used by drivestraight command..
"""
self.robotDrive.drive(outputmag, curve)
def driveStraight(self, speed):
"""driveStraight: invoked from AutoDriveStraight..
"""
# NB: maxOuput isn't applied via set so
# speed is supposedly measured in rpm but this depends on our
# initialization establishing encoding ticks per revolution.
# This is approximate so we rely on the observed values above.
# (DEFAULT_SPEED_MAX_OUTPUT)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = speed / self.maxSpeed
rotateValue = 0
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def startAutoTurn(self, degrees):
self.robot.info("start autoturn from %d to %d" %
(self.getHeading(), degrees))
self.turnPID.reset()
self.turnPID.setSetpoint(degrees)
self.turnPID.enable()
def endAutoTurn(self):
if self.turnPID.isEnabled():
self.turnPID.disable()
def isAutoTurning(self):
return self.turnPID.isEnabled()
def isAutoTurnFinished(self):
return self.turnPID.onTarget()
def turn(self, speed):
"""turn takes a speed, not an angle...
A negative speed is interpretted as turn left.
Note that we bypass application of setMaxOutput Which
only applies to calls to robotDrive.
"""
# In order to turn left, we want the right wheels to go
# forward and left wheels to go backward (cf: tankDrive)
# Oddly, our right master motor is reversed, so we compensate here.
# speed < 0: turn left: rightmotor(negative) (forward),
# leftmotor(negative) (backward)
# speed > 0: turn right: rightmotor(positive) (backward)
# leftmotor(positive) (forward)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = 0
rotateValue = speed / self.maxSpeed
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def trackVision(self):
"""presumed called by either autonomous or teleop commands to
allow the vision subsystem to guide the robot
"""
if self.visionState.DriveLockedOnTarget:
self.stop()
else:
if self.isAutoTurning():
if self.isAutoTurnFinished():
self.endAutoTurn()
self.visionState.DriveLockedOnTarget = True
else:
# setup for an auto-turn
h = self.getCurrentHeading()
tg = self.getTargetHeading(h)
self.startAutoTurn(tg)
def getCurrentHeading(self):
""" getCurrentHeading returns a value between -180 and 180
"""
return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi
def scaleThrottle(self, rawThrottle):
""" scaleThrottle:
returns a scaled value between MIN_THROTTLE_SCALE and 1.0
MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation
Scale the joystick values by throttle before passing to the driveTrain
+1=bottom position; -1=top position
"""
# Throttle in the range of -1 to 1. We would like to change that to a
# range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle
# value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1))
# Then, add 1 to make the range 0-2 rather than -1 to +1
# Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale)
# Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1
#
# Check the results are in the range of k_minThrottleScale to 1, and clip
# it in case the math went horribly wrong.
result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale
if result < self.k_minThrottleScale:
# Somehow our math was wrong. Our value was too low, so force it to the minimum
result = self.k_mintThrottleScale
elif result > 1:
# Somehow our math was wrong. Our value was too high, so force it to the maximum
result = 1.0
return result
def modifyTurnSpeed(self, speedUp):
if speedUp:
self.turnMultiplier = self.k_mediumTurn
else:
self.turnMultiplier = self.k_slowTurn
def inchesToTicks(self, inches):
return int(self.k_quadTicksPerInch * inches)
def destinationReached(self, distance):
return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \
math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
def __init__(self, channel):
self.t = CANTalon(channel)
self.channel = channel
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
class IntakeLauncher(Subsystem):
"""A class to manage/control all aspects of shooting boulders.
IntakeLauncher is comprised of:
aimer: to raise & lower the mechanism for ball intake and shooting
wheels: to suck or push the boulder
launcher: to push boulder into the wheels during shooting
limit switches: to detect if aimer is at an extreme position
potentiometer: to measure our current aim position
boulderSwitch: to detect if boulder is present
Aiming is controlled via two modes
1: driver/interactive: aimMotor runs in VBUS mode to change angle
2: auto/vision control: aimMotor runs in closed-loop position mode
to arrive at a designated position
"""
k_launchMin = 684.0 # manual calibration value
k_launchMinDegrees = -11 # ditto (vestigial)
k_launchMax = 1024.0 # manual calibration value
k_launchMaxDegrees = 45 # ditto (vestigial)
k_launchRange = k_launchMax - k_launchMin
k_launchRangeDegrees = k_launchMaxDegrees - k_launchMinDegrees
k_aimDegreesSlop = 2 # TODO: tune this
k_intakeSpeed = -0.60 # pct vbux
k_launchSpeedHigh = 1.0 # pct vbus
k_launchSpeedLow = 0.7
k_launchSpeedZero = 0
k_servoLeftLaunchPosition = 0.45 # servo units
k_servoRightLaunchPosition = 0.65
k_servoLeftNeutralPosition = 0.75
k_servoRightNeutralPosition = 0.4
k_aimMultiplier = 0.5
k_maxPotentiometerError = 5 # potentiometer units
# launcher positions are measured as a percent of the range
# of the potentiometer..
# intake: an angle approriate for intaking the boulder
# neutral: an angle appropriate for traversing the lowbar
# travel: an angle appropriate for traversing the rockwall, enableLimitSwitch
# highgoal threshold: above which we shoot harder
k_launchIntakeRatio = 0.0
k_launchTravelRatio = 0.51
k_launchNeutralRatio = 0.51
k_launchHighGoalThreshold = 0.69
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
self.launchMin = self.k_launchMin
self.launchMax = self.k_launchMax
self.launchRange = self.launchMax - self.launchMin
self.controlMode = None
self.visionState = robot.visionState
self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId)
self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeLeftMotor.reverseSensor(True)
self.intakeLeftMotor.setSafetyEnabled(False)
self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId)
self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeRightMotor.setSafetyEnabled(False)
self.aimMotor = CANTalon(robot.map.k_IlAimMotorId)
# LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor)
if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot):
self.aimMotor.setSafetyEnabled(False)
self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot)
self.aimMotor.enableLimitSwitch(True, True)
self.aimMotor.enableBrakeMode(True)
self.aimMotor.setAllowableClosedLoopErr(5)
self.aimMotor.configPeakOutpuVoltage(12.0, -12.0)
self.aimMotor.setVoltageRampRate(150)
# TODO: setPID if needed
self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort)
self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort)
self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
def initDefaultCommand(self):
self.setDefaultCommand(ILCmds.DriverControlCommand(self.robot))
def updateDashboard(self):
pass
def beginDriverControl(self):
self.setControlMode("vbus")
def driverControl(self, deltaX, deltaY):
if self.robot.visionState.wantsControl():
self.trackVision()
else:
self.setControlMode("vbus")
self.aimMotor.set(deltaY * self.k_aimMultiplier)
def endDriverControl(self):
self.aimMotor.disableControl()
# ----------------------------------------------------------------------
def trackVision(self):
if not self.visionState.TargetAcquired:
return # hopefully someone is guiding the drivetrain to help acquire
if not self.visionState.LauncherLockedOnTarget:
if math.fabs(self.visionState.TargetY - self.getAngle()) < self.k_aimDegreesSlop:
self.visionState.LauncherLockedOnTarget = True
else:
self.setPosition(self.degreesToTicks(self.TargetY))
elif self.visionState.DriveLockedOnTarget:
# here we shoot and then leave AutoAimMode
pass
else:
# do nothing... waiting form driveTrain to reach orientation
pass
def setControlMode(self, mode):
if mode != self.controlMode:
self.controlMode = mode
if mode == "vbus":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.aimMotor.enableControl()
elif mode == "position":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.Position)
self.aimMotor.enableControl()
elif mode == "disabled":
self.aimMotor.disableControl()
else:
self.robot.error("ignoring controlmode " + mode)
self.controlMode = None
self.aimMotor.disableControl()
# launcher aim -------------------------------------------------------------
def getPosition(self):
return self.aimMotor.getPosition()
def getAngle(self):
pos = self.getPosition()
return self.ticksToDegress(pos)
def setPosition(self, pos):
self.setControlMode("position")
self.aimMotor.set(pos)
def isLauncherAtTop(self):
return self.aimMotor.isFwdLimitSwitchClosed()
def isLauncherAtBottom(self):
return self.aimMotor.isRevLimitSwitchClosed()
def isLauncherAtIntake(self):
if self.k_intakeRatio == 0.0:
return self.isLauncherAtBottom()
else:
return self.isLauncherNear(self.getIntakePosition())
def isLauncherAtNeutral(self):
return self.isLauncherNear(self.getNeutralPosition())
def isLauncherAtTravel(self):
return self.isLauncherNear(self.getTravelPosition())
def isLauncherNear(self, pos):
return math.fabs(self.getPosition() - pos) < self.k_maxPotentiometerError
def getIntakePosition(self):
return self.getLauncherPositionFromRatio(self.k_launchIntakeRatio)
def getNeutralPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchNeutralRatio)
def getTravelPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchTravelRatio)
def getLauncherPositionFromRatio(self, ratio):
return self.launchMin + ratio * self.launchRange
def degressToTicks(self, deg):
ratio = (deg - self.k_launchMinDegrees) / self.k_launchRangeDegrees
return self.k_launchMin + ratio * self.k_launchRange
def ticksToDegrees(self, t):
ratio = (t - self.k_launchMin) / self.k_launchRange
return self.k_launchMinDegrees + ratio * self.k_launchRangeDegrees
def calibratePotentiometer(self):
if self.isLauncherAtBottom():
self.launchMin = self.getPosition()
elif self.isLauncherAtTop():
self.launchMax = self.getPosition()
self.launchRange = self.launchMax - self.launchMin
# intake wheels controls ---------------------------------------------------
def setWheelSpeedForLaunch(self):
if self.isLauncherAtTop():
speed = self.k_launchSpeedHigh
else:
speed = self.k_launchSpeedLow
self.setSpeed(speed)
def setWheelSpeedForIntake(self):
self.setSpeed(self.k_intakeSpeed)
def stopWheels(self):
self.setSpeed(k_launchSpeedZero)
def setWheelSpeed(self, speed):
self.intakeLeftMotor.set(speed)
self.intakeRightMotor.set(-speed)
# boulder and launcher servo controls --------------------------------------------------
def hasBoulder(self):
return self.boulderSwitch.get()
def activateLauncherServos(self):
self.robot.info("activateLauncherServos at:" + self.getAimPosition())
self.launcherServoLeft.set(self.k_servoLeftLaunchPosition)
self.launcherServoRight.set(self.k_servoRightLaunchPosition)
def retractLauncherServos(self):
self.launcherServoLeft.set(self.k_servoLeftNeutralPosition)
self.launcherServoRight.set(self.k_servoRightNeutralPosition)
class Choppers:
chopper_motor = CANTalon
def __init__(self):
self.chopper_motor = CANTalon(motor_map.chopper_motor)
self.chopper_motor.setSafetyEnabled(False)
self.chopper_motor.enableLimitSwitch(True, True)
def choppers_up(self):
self.chopper_motor.set(1)
def chopper_down(self):
self.chopper_motor.set(-1)
def chopper_off(self):
self.chopper_motor.set(0)
def execute(self):
if float(self.chopper_motor.getOutputCurrent()) > 10.0: # Safety check
self.chopper_motor.set(0.0)
def safety_check(self):
self.execute()
