def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.driveStyle = "Tank"
SmartDashboard.putString("DriveStyle", self.driveStyle)
#SmartDashboard.putData("Mode", self.mode)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
self.zero()
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
self.k = 1
self.sensitivity = 1
SmartDashboard.putNumber("K Value", self.k)
SmartDashboard.putNumber("sensitivity", self.sensitivity)
self.prevLeft = 0
self.prevRight = 0
def initialize(self, robot):
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
if map.robotId == map.astroV1:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10, 0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backBottomSensor)
self.frontSwitch = DigitalInput(map.frontBottomSensor)
self.upSwitch = DigitalInput(map.upSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = 0 #degrees
self.climbSpeed = 0.5
self.wheelSpeed = 0.5
self.backHold = -0.15 #holds back stationary if extended ADJUST**
self.frontHold = -0.1 #holds front stationary if extended
self.kP = 0.4 #proportional gain for angle to power
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False