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)