class Chassis(Subsystem):
def __init__(self):
super().__init__("Chassis")
self.spark_L1 = Spark(robotmap.spark_L1)
self.spark_L2 = Spark(robotmap.spark_L2)
self.spark_R1 = Spark(robotmap.spark_R1)
self.spark_R2 = Spark(robotmap.spark_R2)
self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2)
self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2)
self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R)
self.gyro = ADXRS450_Gyro(robotmap.gyro)
self.encoder_L = Encoder(0, 1)
self.encoder_R = Encoder(2, 3)
self.dist_pulse_L = pi * 6 / 2048
self.dist_pulse_R = pi * 6 / 425
@classmethod
def setDriveSpd(cls, spd_drive_new):
robotmap.spd_chassis_drive = spd_drive_new
@classmethod
def setRotateSpd(cls, spd_rotate_new):
robotmap.spd_chassis_rotate = spd_rotate_new
def stop(self):
self.drive.stopMotor()
def curvatureDrive(self, spd_x, spd_z):
self.drive.curvatureDrive(spd_x, spd_z, True)
def joystickDrive(self):
self.drive.curvatureDrive(
-(oi.joystick.getRawAxis(1)) * robotmap.spd_chassis_drive,
oi.joystick.getRawAxis(4) * robotmap.spd_chassis_rotate, True)
def setupEncoder(self):
self.encoder_L.setDistancePerPulse(self.dist_pulse_L)
self.encoder_R.setDistancePerPulse(self.dist_pulse_R)
self.encoder_L.reset()
self.encoder_R.reset()
def getGyroAngle(self):
return self.gyro.getAngle()
def resetGyro(self):
self.gyro.reset()
def gyroDrive(self, spd_temp, amt_turn=None):
if spd_temp and amt_turn is not None:
self.curvatureDrive(spd_temp, amt_turn)
elif amt_turn is None:
self.curvatureDrive(spd_temp, 0.0)
else:
raise ("GyroDrive() failed!")
class DriveArcade(wpilib.command.Command):
""" Command to control drive using gamepad in arcade mode. """
diffDrive: DifferentialDrive
gamePad: GenericHID
nominal: float
def __init__(self, drive: Drive, leftMotors: SpeedControllerGroup, rightMotors: SpeedControllerGroup, gamePad: GenericHID):
super().__init__("DriveArcade")
self.requires(drive)
self.gamePad = gamePad
self.diffDrive = DifferentialDrive(leftMotors, rightMotors)
self.nominal = 3.0 / 8.0
self.deadband = 0.15 # XBox360 controller has a lot of slop
def tweak(self, val, scale):
tweaked: float = DifferentialDrive.applyDeadband(val, self.deadband)
if tweaked == 0.0:
# User input value in deadband, just return 0.0
return tweaked
tweaked *= scale * (1 - self.nominal)
if tweaked > 0:
tweaked = (tweaked * tweaked) + self.nominal
else:
tweaked = (tweaked * -tweaked) - self.nominal
return tweaked
def initialize(self):
self.diffDrive.setSafetyEnabled(True)
def execute(self):
throttle: float = self.tweak(-self.gamePad.getY(GenericHID.Hand.kLeft), 1.0)
rotation: float = self.tweak(self.gamePad.getX(GenericHID.Hand.kRight), 0.5)
#print("Throttle", throttle, "Rotation", rotation)
self.diffDrive.arcadeDrive(throttle, rotation, False)
def isFinished(self):
return False
def interrupted(self):
self.diffDrive.stopMotor()
self.diffDrive.setSafetyEnabled(False)
def end(self):
self.interrupted()
class driveTrain():
def __init__(self, robot):
self.operate = operatorFunctions(drive=self,robot=robot)
self.gyro = AHRS.create_spi()
#self.gyro = wpilib.interfaces.Gyro()
"""Sets drive motors to a cantalon or victor"""
self.instantiateMotors()
self.instantiateEncoders()
#self.encoderReset()
#self.driveBase = arcadeDrive()
self.shifter = wpilib.DoubleSolenoid(51, 0, 1)#Initilizes the shifter's solenoid and sets it to read fron digital output 0
self.lfMotor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.lbMotor = ctre.wpi_victorspx.WPI_VictorSPX(11)
self.rfMotor = ctre.wpi_victorspx.WPI_VictorSPX(9)
self.rbMotor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.lfMotor.setNeutralMode(2)
self.lbMotor.setNeutralMode(2)
self.rfMotor.setNeutralMode(2)
self.rbMotor.setNeutralMode(2)
self.left = wpilib.SpeedControllerGroup(self.lfMotor, self.lbMotor)
self.right = wpilib.SpeedControllerGroup(self.rfMotor, self.rbMotor)
self.drive = DifferentialDrive(self.left, self.right)
self.firstTime = True#Check for autonDriveStraight
self.firstRun = True#Check for autonPivot
self.resetFinish = False#Check for encoder reset
self.setWheelCircumference()
self.oldGyro = 0
self.moveNumber = 1
self.shiftCounter = 0
def setWheelCircumference(self):
self.wheelCircumference = 18.84954
def instantiateEncoders(self):
self.lfMotor.configSelectedFeedbackSensor(0, 0, 0)
self.rbMotor.configSelectedFeedbackSensor(0, 0, 0)
self.lfMotor.setSensorPhase(True)
def instantiateMotors(self):
self.lfMotor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.lbMotor = ctre.wpi_victorspx.WPI_VictorSPX(11)
self.rfMotor = ctre.wpi_victorspx.WPI_VictorSPX(9)
self.rbMotor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.lfMotor.setNeutralMode(2)
self.lbMotor.setNeutralMode(2)
self.rfMotor.setNeutralMode(2)
self.rbMotor.setNeutralMode(2)
def drivePass(self, leftY, rightX, leftBumper, rightBumper, aButton):
self.arcadeDrive(leftY, rightX)
self.shift(leftBumper, rightBumper)
self.manualEncoderReset(aButton)
pass
def scaleInputs(self, leftY, rightX):
if abs(leftY) < .05:
return .75 * rightX
rightX = (-(math.log10((2*(abs(leftY)))+1)-1)*rightX)
return rightX
def printer(self):
print('Why does Hescott look so much like shrek?')
def arcadeDrive(self, leftY, rightX):
#self.drive.arcadeDrive(leftY * -.82, self.scaleInputs(leftY, rightX) * .82)
#print((-1 * self.scaleInputs(leftY, rightX)))
#self.drive.arcadeDrive((-1 * self.scaleInputs(leftY, rightX)), (rightX/2))
self.drive.arcadeDrive(leftY * -.82, rightX * .82)
def shift(self, leftBumper, rightBumper):
#print(self.shifter.get())
if leftBumper:#When left bumper is pressed we shift gears
if self.shifter.get() == 1:#Checks to see what gear we are in and shifts accordingly
self.shifter.set(2)
#print("shifting left")
if rightBumper:
if self.shifter.get() == 2 or self.shifter.get() == 0:
self.shifter.set(1)
#print("shifting right")
def getGyroAngle(self):
return (self.gyro.getAngle()-self.oldGyro)
def zeroGyro(self):
self.gyro.reset()
while(abs(self.getGyroAngle()) > 340):
self.gyro.reset()
def encoderReset(self):
self.lfMotor.setQuadraturePosition(0, 0)
self.rbMotor.setQuadraturePosition(0, 0)
#print("Encoders Reset")
def printEncoderPosition(self):
lfEncoder = -(self.lfMotor.getQuadraturePosition())
rbEncoder = self.rbMotor.getQuadraturePosition()
averageEncoders = (lfEncoder + rbEncoder) / 2
#print(averageEncoders)
#print(rbEncoder)
def manualEncoderReset(self, aButton):
if aButton:
self.lfMotor.setQuadraturePosition(0, 0)
self.rbMotor.setQuadraturePosition(0, 0)
else:
pass
def autonShift(self, gear):
if gear == 'low':
if self.shifter.get() == 1 or self.shifter.get() == 0:
self.shifter.set(2)
#print('Shift to low')
#return False
elif gear == 'high':
if self.shifter.get() == 2 or self.shifter.get() == 0:
self.shifter.set(1)
#print('Shift to high')
#return False
else:
pass
def autonPivot(self, turnAngle, turnSpeed):
slowDownSpeed = .14
correctionDeadzone = .5
if self.firstRun:
self.oldGyro = self.gyro.getAngle()
self.firstRun = False
turnSpeed -= (2*turnSpeed/(1+math.exp(0.045*(-1 if turnAngle>0 else 1)*(-turnAngle + self.getGyroAngle()))))
turnSpeed = max(turnSpeed, slowDownSpeed)
#turnSpeed = 0.15
'''
if abs(turnAngle - self.getGyroAngle()) < 25:
#print('Gyro Angle:'+str(self.getGyroAngle()))
#print('Turn Speed:'+str(turnSpeed))
turnSpeed = slowDownSpeed
'''
if turnAngle < 0:
if abs(turnAngle - self.getGyroAngle()) > correctionDeadzone:
if self.getGyroAngle() >= turnAngle:
self.drive.tankDrive(-(turnSpeed) * .82, (turnSpeed) * .82,False)
#print('Turning Left')
return True
elif self.getGyroAngle() <= turnAngle:
self.drive.tankDrive(.2, -.2)
return True
else:
pass
else:
#print("Done Turning Left")
print(self.getGyroAngle())
self.drive.stopMotor()
self.firstRun = True
return False
elif turnAngle > 0:
if abs(turnAngle - self.getGyroAngle()) > correctionDeadzone:
if self.getGyroAngle() <= turnAngle:
#print('Turning Right')
self.drive.tankDrive((turnSpeed) * .82, -(turnSpeed) * .82,False)
return True
elif self.getGyroAngle() >= turnAngle:
self.drive.tankDrive(-.2, .2)
return True
else:
pass
else:
#print('Done Turning Right')
#print(self.getGyroAngle())
self.drive.stopMotor()
self.firstRun = True
return False
def autonDriveStraight(self, speed, distance):
#print('entered auton straight')
lSpeed = speed
rSpeed = speed
encoderDistance = (distance / self.wheelCircumference) * 5887#Figueres out how far to spin the wheels in encoder codes, 265 is how many pins on current encoders
#print('Encoder Distance' + str(encoderDistance))
if self.firstTime:#Checks for first time through the function to only reset encoders on the first time
#print('passed first check')#Debugging
#self.encoderReset()#Resets encoders
self.oldGyro = self.gyro.getAngle()
self.oldPositionLeft = -(self.lfMotor.getQuadraturePosition())
self.oldPositionRight = self.rbMotor.getQuadraturePosition()
self.autonCounter = 0
self.firstTime = False
self.lfEncoderPosition = -(self.lfMotor.getQuadraturePosition()) - self.oldPositionLeft
self.rbEncoderPosition = self.rbMotor.getQuadraturePosition() - self.oldPositionRight
#print(self.lfEncoderPosition)
#print(self.rbEncoderPosition)
averageEncoders = (self.lfEncoderPosition + self.rbEncoderPosition) / 2
#print('Average Encodes' + str(averageEncoders))
'''
if averageEncoders > 250 and not self.resetFinish:
#self.encoderReset()
self.printEncoderPosition()
return True
else:
if not self.resetFinish:
print(self.lfEncoderPosition)
print(self.rbEncoderPosition)
#print('Encoder Reset Finished')
self.resetFinish = True
'''
if averageEncoders < encoderDistance and self.autonCounter == 0:
speedAdjustment = .05
slowDownSpeed = .25
gyroAngle = self.getGyroAngle();
speedAdjustment /= 1+math.exp(-gyroAngle)
speedAdjustment -= 0.025
rSpeed += speedAdjustment
lSpeed -= speedAdjustment
'''
if self.getGyroAngle() < -1:
rSpeed = rSpeed - speedAdjustment
elif self.getGyroAngle() > 1:
lSpeed = lSpeed - speedAdjustment
'''
if averageEncoders > encoderDistance - 500:
lSpeed = slowDownSpeed
rSpeed = slowDownSpeed
#print('Slowing Down')
self.drive.tankDrive(lSpeed * .82, rSpeed * .82,False)
return True
else:
if self.autonCounter < 4:
#print('Active Breaking')
self.drive.tankDrive(-.15 * .82, -.15 * .82,False)
self.autonCounter = self.autonCounter + 1
return True
else:
#print('EndLoop')
self.resetFinish = False
self.firstTime = True
self.drive.stopMotor()
#print(self.lfEncoderPosition)
print(self.rbEncoderPosition)
return False
'''
def autonAngledTurn(self, turnAngle):#Angle is in degrees
ROBOT_WIDTH = 24.3
def getSpeeds(self, angle, radius, speed=1):
return [speed, speed*(lambda x: x[1]/x[0])(getDistances(angle, radius))]
def getDistances(self, angle, radius):
return [(radius + ROBOT_WIDTH/2)*math.radians(angle), (radius - ROBOT_WIDTH/2)*math.radians(angle) ]
'''
def autonMove(self, moveNumberPass, commandNumber, speed, distance, turnAngle, turnSpeed):
if moveNumberPass == self.moveNumber:
#print(self.moveNumber)
if commandNumber == 0:
if self.autonDriveStraight(speed, distance):
pass
else:
#print(self.getGyroAngle())
#print('Move ' + str(moveNumberPass) + ' Complete')
self.moveNumber = moveNumberPass + 1
elif commandNumber == 1:
if self.autonPivot(turnAngle, turnSpeed):
pass
else:
#print(self.getGyroAngle())
#print('Move ' + str(moveNumberPass) + ' Complete')
self.moveNumber = moveNumberPass + 1
elif commandNumber == 2:
pass
else:
#print('3rd pass')
pass
def resetMoveNumber(self):
self.moveNumber = 1
class Robot(wpilib.IterativeRobot):
def __init__(self):
super().__init__()
# Initiate up robot drive
self.left_motor = wpilib.Spark(LEFT_PORT)
self.right_motor = wpilib.Spark(RIGHT_PORT)
self.drive = DifferentialDrive(self.left_motor, self.right_motor)
self.drive.setExpiration(.1)
# Initiate arm and lift
self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT)
self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2)
# Initiate button stuff
self.buttons = set()
self.button_toggles = None
self.pressed_buttons = {}
self.rumble_time = None
self.rumble_length = RUMBLE_LENGTH
self.reset_buttons()
# Initiate gamepad
self.game_pad = wpilib.Joystick(GAMEPAD_NUM)
self.max_enhancer = 0
self.smart_dashboard = wpilib.SmartDashboard
# auto_chooser = wpilib.SendableChooser()
# auto_chooser.addDefault("Enable Auto", "Enable Auto")
# auto_chooser.addObject("Disable Auto", "Disable Auto")
self.smart_dashboard.putBoolean("Enable Auto", True)
self.auto_start_time = None
self.current_speed = [0, 0]
self.last_tank = 0
self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel
self.tank_dir = None
def reset_buttons(self):
"""Resets the values of the button toggles to default likewise
defined here
"""
self.button_toggles = {
"REVERSE": False,
"LOCK": True,
"STOP": False,
"SPEED": False,
"GRAB": False
}
self.pressed_buttons = {}
self.rumble_time = None
def stop(self):
self.reset_buttons()
self.last_tank = get_millis()
self.drive.stopMotor()
self.robot_lift.stopMotor()
self.robot_lift_2.stopMotor()
self.current_speed = [0, 0]
# Events
def robotInit(self):
"""Runs at the same time as __init__. Appears to be redundant in this
case
"""
self.stop()
def robotPeriodic(self):
pass
def disabledInit(self):
self.stop()
def disabledPeriodic(self):
self.stop()
def autonomousInit(self):
self.stop()
self.auto_start_time = None
self.gyro.calibrate() # Takes five seconds
self.stop() # Have to reset tank time to correct acceleration
def tank(self, left, right, accel=None):
if left == right:
if isinstance(self.tank_dir, type(None)):
self.tank_dir = self.gyro.getAngle()
else:
self.tank_dir = None
turn = None
current_angle = None
if not isinstance(self.tank_dir, type(None)):
current_angle = self.gyro.getAngle()
if current_angle < self.tank_dir - ANGLE_MARGIN:
turn = "right"
elif current_angle > self.tank_dir + ANGLE_MARGIN:
turn = "left"
if isinstance(accel, type(None)):
accel = ACCEL
speed1 = self.current_speed[0]
speed2 = self.current_speed[1]
if not isinstance(turn, type(None)) and GYRO_ENABLE:
if turn == "left":
left -= ANGLE_CHANGE
right += ANGLE_CHANGE
elif turn == "right":
right -= ANGLE_CHANGE
left += ANGLE_CHANGE
print((round(self.tank_dir, 3)
if not isinstance(self.tank_dir, type(None)) else None),
(round(current_angle, 3)
if not isinstance(current_angle, type(None)) else None),
round(left, 3), round(right, 3), turn)
if .4 > speed1 > 0:
if left <= 0:
speed1 = 0
else:
speed1 = .4
if -.4 < speed1 < 0:
if left >= 0:
speed1 = 0
else:
speed1 = -.4
if .4 > speed2 > 0:
if right <= 0:
speed2 = 0
else:
speed2 = .4
if -.4 < speed2 < 0:
if right >= 0:
speed2 = 0
else:
speed2 = -.4
if isinstance(accel, type(None)):
self.drive.tankDrive(left, right)
return None
vel_change = ((get_millis() - self.last_tank) / 1000) * accel
if abs(speed1 - left) < WHEEL_LOCK:
speed1 = left
else:
if speed1 < left:
speed1 += vel_change
elif speed1 > left:
speed1 -= vel_change
if abs(speed2 - right) < WHEEL_LOCK:
speed2 = right
else:
if speed2 < right:
speed2 += vel_change
elif speed2 > right:
speed2 -= vel_change
self.current_speed = [speed1, speed2]
# print([round(left, 2), round(right, 2)], [round(speed1, 2), round(speed2, 2)], round(vel_change, 4))
self.drive.tankDrive(*self.current_speed)
# print([round(x, 2) for x in self.current_speed], round(vel_change, 2), accel)
self.last_tank = get_millis()
def autonomousPeriodic(self):
if self.smart_dashboard.getBoolean("Auto Enabled", True):
if not isinstance(self.auto_start_time, type(None)):
# Auto enabled and running
# percent = (get_millis() - self.auto_start_time) / AUTO_DUR
# if percent < .5:
# speed = (percent/.5) * AUTO_SPEED
# elif percent < 1:
# speed = ((1 - percent)/.5) * AUTO_SPEED
# else:
# speed = 0
if get_millis() >= self.auto_start_time + AUTO_DUR / 2:
self.tank(0, 0, AUTO_ACCEL)
else:
self.tank(AUTO_SPEED, AUTO_SPEED, AUTO_ACCEL)
else:
# Auto enabled and not started
self.auto_start_time = get_millis()
def teleopInit(self):
self.stop()
self.drive.setSafetyEnabled(True)
def set_rumble(self, left=True, value=1, length=RUMBLE_LENGTH):
"""Sets given rumble to given value"""
if not left:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kLeftRumble,
value)
else:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kRightRumble,
value)
self.rumble_time = get_millis()
self.rumble_length = length
def check_rumble(self):
"""Checks if rumbling has surpassed RUMBLE_LENGTH"""
if not isinstance(self.rumble_time, type(None)) and get_millis(
) > self.rumble_time + self.rumble_length:
# This rumbling has gone on long enough!
self.set_rumble(True, 0)
self.set_rumble(False, 0)
self.rumble_time = None
def execute_buttons(self):
"""Check whether each button was just pressed and updates relevant
toggle
"""
arms = 0
button_states = self.get_buttons()
for button_name in BUTTON_PORTS:
port = BUTTON_PORTS[button_name]
angle = -1
if isinstance(port, list):
angle = port[1]
port = port[0]
if port in button_states and button_states[port] is True:
# button was just pressed
if button_name in self.button_toggles:
# needs toggled
if button_name == "GRAB":
pass
# print(button_name, port == POV,
# self.get_raw_buttons()[POV], angle)
if not (port == POV
and not self.get_raw_buttons()[POV] == angle):
new_state = not self.button_toggles[button_name]
self.button_toggles[button_name] = new_state
self.set_rumble(new_state)
# print(button_name, new_state)
elif self.get_raw_buttons()[POV] == angle:
# Button angle correct, check button name
if button_name == "INC SPEED":
self.max_enhancer += SPEED_ADJUST
self.set_rumble(True, 1)
elif button_name == "DEC SPEED":
self.max_enhancer -= SPEED_ADJUST
self.set_rumble(False, 1)
elif button_name == "RES SPEED":
self.max_enhancer = 0
self.set_rumble(True, length=200)
elif port in button_states:
# BUTTON BEING PRESSED
if button_name == "ARM IN":
arms = max(-1, min(-(ARM_SPEED_IN), 1))
self.button_toggles["GRAB"] = False
elif button_name == "ARM OUT":
arms = max(-1, min(ARM_SPEED_OUT, 1))
self.button_toggles["GRAB"] = False
# if arms == 0 and self.button_toggles["GRAB"]:
# self.robot_lift_2.set(-ARM_SPEED_IN)
# else:
# self.robot_lift_2.set(arms)
self.check_rumble()
def execute_axes(self):
"""Checks each axis and updates attached motor"""
axes = self.get_axes()
if not self.button_toggles["STOP"]:
self.tank(axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS])
# self.left_motor.set(axes[LEFT_WHEELS_AXIS])
# self.right_motor.set(axes[RIGHT_WHEELS_AXIS])
left_trigger = axes[LIFT_AXIS1]
right_trigger = axes[LIFT_AXIS2]
if left_trigger > 0:
if right_trigger <= 0:
self.robot_lift.set(-left_trigger)
self.robot_lift_2.set(-left_trigger)
elif right_trigger > 0:
self.robot_lift.set(right_trigger)
# print(right_trigger)
else:
self.robot_lift.set(0)
self.robot_lift_2.set(0)
def teleopPeriodic(self):
"""Runs repeatedly while robot is in teleop"""
self.execute_buttons()
self.execute_axes()
def get_raw_axes(self, _round=False):
"""Return a dictionary of controller axes"""
i = 0
axes = {}
while True:
try:
value = self.game_pad.getRawAxis(i)
if i == RIGHT_WHEELS_AXIS:
if value < 0:
value /= .9 # Right stick does not go full forward
value = min(value, 1)
if _round:
axes[i] = round(value, 2)
else:
axes[i] = value
except IndexError:
break
i += 1
return axes
def get_axes(self, _round=False):
"""Returns the current axes and values with deadzones and scaled"""
axes = self.get_raw_axes(_round)
# Correct deadzones and scale
for axis in axes:
if axis != LIFT_AXIS1 and axis != LIFT_AXIS2:
if axis in DEADZONES:
value = axes[axis]
neg = -1 if value < 0 else 1
deadzone = DEADZONES[axis]
if abs(value) > deadzone:
value -= deadzone * neg
value /= 1 - deadzone
if not self.button_toggles[
"SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[axis][value >= 0] +
self.max_enhancer, 1))
else:
value = 0
if REVERSES[axis]:
value *= -1
axes[axis] = value
else: # TODO: Clean up
# TRIGGERS FOR LIFT
value = axes[axis]
deadzone = DEADZONES[LIFT_AXIS1]
if value > deadzone:
value -= deadzone
value /= 1 - deadzone
if not self.button_toggles["SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[LIFT_AXIS1][axis == LIFT_AXIS1] +
self.max_enhancer, 1))
if self.button_toggles["REVERSE"]:
axes[LEFT_WHEELS_AXIS] *= -1
axes[RIGHT_WHEELS_AXIS] *= -1
dif, avg = None, None
if self.button_toggles["LOCK"]:
left = axes[LEFT_WHEELS_AXIS]
right = axes[RIGHT_WHEELS_AXIS]
dif = abs(abs(left) - abs(right))
if dif <= WHEEL_LOCK and left != 0 and right != 0:
lneg = left < 0
rneg = right < 0
smaller = min(abs(left), abs(right))
avg = smaller + dif / 2
axes[LEFT_WHEELS_AXIS] = avg
axes[RIGHT_WHEELS_AXIS] = avg
if lneg:
axes[LEFT_WHEELS_AXIS] *= -1
if rneg:
axes[RIGHT_WHEELS_AXIS] *= -1
# print(self.button_toggles["LOCK"], left, right,
# axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS], dif, avg)
return axes
def get_raw_buttons(self):
"""Return a dictionary of raw button states"""
i = 1 # Controller buttons start at 1
buttons = {}
while i <= 10:
# Gets ten buttons or stops at index error <-- shouldn't happen
try:
buttons[i] = self.game_pad.getRawButton(i)
except IndexError:
break
i += 1
buttons[i] = self.game_pad.getPOV(0)
return buttons
def get_buttons(self):
"""Returns a dictionary of bools representing whether each button was
just pressed
"""
raw_buttons = self.get_raw_buttons()
for button in raw_buttons:
being_pressed = (
not raw_buttons[button] is False) and raw_buttons[button] != -1
if button not in self.pressed_buttons and being_pressed:
# If button not already accounted for and being pressed
self.pressed_buttons[button] = True
elif button in self.pressed_buttons:
if being_pressed:
# Being pressed, already used
self.pressed_buttons[button] = False
else:
# Was pressed, no longer being pressed
del self.pressed_buttons[button]
return self.pressed_buttons
class Drive:
def __init__(self):
""" Drive Train """
# drive train motors
self.frontLeftMotor = WPI_VictorSPX(1)
self.frontRightMotor = WPI_VictorSPX(2)
self.rearRightEncoder = WPI_TalonSRX(3)
self.rearLeftEncoder = WPI_TalonSRX(4)
# reverses direction of drive train motors
self.rearRightEncoder.setInverted(True)
self.frontRightMotor.setInverted(True)
# drive train motor groups
self.leftDrive = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftEncoder)
self.rightDrive = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightEncoder)
# setting up differential drive
self.drive = DifferentialDrive(self.leftDrive, self.rightDrive)
""" Pneumatics """
# drive pneumatics
self.gearSolenoid = wpilib.DoubleSolenoid(2, 3)
""" NavX """
self.navx = navx.AHRS.create_spi()
self.navx.reset()
""" PID """
self.setpoint = 0
self.kP = 0.1
self.kI = 0
self.kD = 0
self.integral = 0
self.previousError = 0
self.resetAngle = True
def reset(self):
""" Resetting sensors """
self.rearLeftEncoder.setSelectedSensorPosition(0)
self.rearRightEncoder.setSelectedSensorPosition(0)
self.navx.reset()
def setSetpoint(self, setpoint):
""" Sets setpoint for drive PID """
self.setpoint = setpoint
def setPID(self, kP, kI, kD):
""" Sets PID Variables """
self.kP = kP
self.kI = kI
self.kP = kD
def PID(self):
""" Calcuating next ouput value via PID """
if self.getGearSolenoid() == 2:
self.gearSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
self.error = self.setpoint - (self.navx.getAngle() % 360)
self.integral = self.integral + (self.error * .02)
self.derivative = (self.error - self.previousError) / 0.02
if abs(self.setpoint - (self.navx.getAngle() % 360)) > 180:
self.turnRight = True
elif abs(self.setpoint - (self.navx.getAngle() % 360)) < 180:
self.turnRight = False
self.rcw = (self.kP * self.error) + (self.kI * self.integral) + (
self.kD * self.derivative)
self.rcw = self.rcw * 0.5
def execute(self):
""" Runs PID with current setpoint """
# does not work due to navx not working. see turnAngle() instead
self.PID()
if self.turnRight is True:
self.drive.tankDrive(self.rcw, self.rcw)
elif self.turnRight is False:
self.drive.tankDrive(-self.rcw, -self.rcw)
def turnAngle(self, angle):
""" Turn robot to specified angle value """
# getting encoder position values and averaging
leftEncoderValue = abs(
self.rearLeftEncoder.getSelectedSensorPosition())
rightEncoderValue = abs(
self.rearRightEncoder.getSelectedSensorPosition())
driveTrainEncoderValue = abs(rightEncoderValue + leftEncoderValue) / 2
# converting encoder value to angles
self.targetAngleEncoder = abs((4096 / 90) * angle)
if angle > 0:
self.targetAngleEncoder = self.targetAngleEncoder + 1024
elif angle < 0:
self.targetAngleEncoder = self.targetAngleEncoder + 256
if driveTrainEncoderValue <= self.targetAngleEncoder:
if angle > 0:
self.drive.tankDrive(1, 1)
if angle < 0:
self.drive.tankDrive(-1, -1)
self.resetAngle = False
elif driveTrainEncoderValue >= (self.targetAngleEncoder + 1024):
self.resetAngle = True
else:
self.leftDrive.stopMotor()
self.rightDrive.stopMotor()
def turnToTarget(self, angleLimelight):
""" Turn robot to the limelight target """
if angleLimelight > 15:
nspeed = angleLimelight / 50
self.drive.tankDrive(nspeed + 0.1, nspeed + 0.1)
elif 5 < angleLimelight < 15:
nspeed = angleLimelight / 75
self.drive.tankDrive(nspeed + 0.3, nspeed + 0.3)
elif 2 < angleLimelight < 5:
self.drive.tankDrive(0.4, 0.4)
elif -2 < angleLimelight < 2:
self.drive.stopMotor()
elif -5 < angleLimelight < 2:
self.drive.tankDrive(-0.4, -0.4)
elif -15 < angleLimelight < -5:
nspeed = angleLimelight / 75
self.drive.tankDrive(nspeed - 0.3, nspeed - 0.3)
elif angleLimelight < -15:
nspeed = angleLimelight / 50
self.drive.tankDrive(nspeed - 0.1, nspeed - 0.1)
def changeGear(self, buttonStatus):
""" Switch gear between high and low """
if buttonStatus is True:
# high gear
self.gearSolenoid.set(wpilib.DoubleSolenoid.Value.kForward)
elif buttonStatus is False:
# low gear
self.gearSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
def tankDrive(self, leftJoystickAxis, rightJoystickAxis):
""" Tank drive at set scaling using both joysticks """
scaling = 1
self.drive.tankDrive(-leftJoystickAxis * scaling,
rightJoystickAxis * scaling, True)
def arcadeDrive(self, rightJoystickAxis, rotateAxis):
""" Arcade drive at set scaling using the right joystick """
scaling = 1
self.drive.arcadeDrive(rotateAxis, -rightJoystickAxis * scaling, True)
