class Robot(TimedRobot):
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightSensor1 = SensorCollection(self.RightMotor1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
# self.drive = DifferentialDrive(self.leftDriveMotors,
# self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3, Encoder.EncodingType.k4X)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def disabledInit(self):
pass
def autonomousInit(self):
pass
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
self.dashboard.putNumber("Encoder", self.RightSensor1.getQuadratureVelocity()/2048*60)
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
speed = self.testEncoder.get() / 1028
self.leftDriveMotors.set(speed)
self.rightDriveMotors.set(speed)
def teleopPeriodic(self):
pass
def testPeriodic(self):
pass
class Indexer():
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def update(self):
self.realValue = self.encoder.get()
offset = (self.targetValue - self.realValue) % self.totalValues - (self.totalValues / 2)
self.speed = clamp(abs(offset) * self.speedmult, self.minspeed, self.maxspeed)
self.dashboard.putString("Indexer", "{} offset".format(offset))
if (offset > self.allowedError):
self.motor.set(ControlMode.PercentOutput, -self.speed)
elif (offset < -self.allowedError):
self.motor.set(ControlMode.PercentOutput, self.speed)
else:
self.motor.set(ControlMode.PercentOutput, 0)
if (abs(offset) < 15):
if (self.operator.getIndexUp()):
self.hole = (self.hole + 1) % 5
elif (self.operator.getIndexDown()):
self.hole = (self.hole + 4) % 5
self.setRotation(self.hole * 72 + self.holeOffset)
def getRotation(self) -> float:
return self.realValue / self.totalValues * 360
def setRotation(self, degrees):
self.targetValue = clamp(degrees/360*self.totalValues,
0, self.totalValues)
class Turret(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Turret')
self.robot = robot
self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X)
motors = {}
self.map = self.robot.botMap
self.tmotors = motors
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.tmotors:
self.tmotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast)
self.kP = 0.05
self.kI = 0.000
self.kD = 0.002
self.turretPID = PID(self.kP, self.kI, self.kD)
self.turretPID.limitVal(0.3)
self.setPow = 0
def set(self, pw):
self.tmotors['turret'].set(ctre.ControlMode.PercentOutput, pw)
def setPower(self, pow):
self.setPow = pow
def getEnc(self):
return self.tEncoder.get()
def periodic(self):
if self.robot.Limelight.isExisting():
self.set(0) # self.turretPID.outVal(self.robot.Limelight.getX()))
else:
self.set(self.setPow)
class Flywheel(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Flywheel')
self.CurPos = 0
self.PasPos = 0
self.robot = robot
self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X)
self.map = self.robot.botMap
motor = {}
piston = {}
for name in self.robot.botMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(
self.robot.botMap.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
piston[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.fmotor = motor
self.fpiston = piston
for name in self.fmotor:
self.fmotor[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.fmotor[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Fly']), 40)
self.kP = 0.008
self.kI = 0.00002
self.kD = 0.00018
self.kF = 0.0 # tune me when testing
self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF)
self.flywheelPID.MaxIn(680)
self.flywheelPID.MaxOut(1)
self.flywheelPID.limitVal(0.95) # Limit PID output power to 50%
self.feetToRPS = 51.111
def log(self):
wpilib.SmartDashboard.putNumber('Flywheel Enc', self.Fenc.get())
wpilib.SmartDashboard.putNumber('Flywheel Vel',
self.getVelocity(self.Fenc.get()))
def init(self):
self.Fenc.reset()
def set(self, pow):
self.fmotor['RFly'].set(ctre.ControlMode.PercentOutput, pow)
self.fmotor['LFly'].set(ctre.ControlMode.PercentOutput, pow)
def setVelocityPID(self, rps):
self.flywheelPID.setPoint(rps)
if rps == 0:
pow = 0
else:
pow = self.flywheelPID.outVel(self.getVelocity(self.get()))
return pow
def get(self):
return self.Fenc.get()
def getVelocity(self, input):
self.CurPos = -(input / 1025) * (2 * math.pi)
vel = (self.CurPos - self.PasPos) / 0.02
self.PasPos = self.CurPos
return vel
class Arm(Subsystem):
def __init__(self, robot):
super().__init__("Arm")
self.robot = robot
self.peakCurrentLimit = 30
self.PeaKDuration = 50
self.continuousLimit = 15
motor = {}
for name in self.robot.RobotMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name])
self.motors = motor
for name in self.motors:
self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.motors[name].enableCurrentLimit(True)
self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X)
self.Zero = DigitalInput(6)
self.kp = 0.00035
self.ki = 0.00000000001
self.kd = 0.0000001
self.ArmPID = PID(self.kp, self.ki, self.kd)
def log(self):
wpilib.SmartDashboard.putNumber('armEnc', self.getHeight())
wpilib.SmartDashboard.putNumber('Zero', self.getZeroPos())
"""
Get Functions
"""
def getHeight(self):
# get encoder values
return self.AEnc.get()
def getZeroPos(self):
# get zero position of arm
return self.Zero.get()
"""
set functions
"""
def set(self, power):
self.motors['RTArm'].set(ctre.ControlMode.PercentOutput, power)
self.motors['LTArm'].set(ctre.ControlMode.PercentOutput, power)
def stop(self):
self.set(0)
def resetHeight(self):
self.AEnc.reset()
class Drive(Subsystem):
distPID = 0
anglePID = 0
prevDist = [0, 0]
maxSpeed = 0.85
yaw = 0
pitch = 0
roll = 0
leftVal = 0
rightVal = 0
leftConv = 6 / 12 * math.pi / 256
rightConv = -6 / 12 * math.pi / 256
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 setGains(self, p, i, d, f):
self.distController.setPID(p, i, d, f)
def setGainsAngle(self, p, i, d, f):
self.angleController.setPID(p, i, d, f)
def periodic(self):
self.updateSensors()
def __getDistance__(self):
return self.getAvgDistance()
def __setDistance__(self, output):
self.distPID = output
def __getAngle__(self):
return self.getAngle()
def __setAngle__(self, output):
self.anglePID = output
def setMode(self, mode, name=None, distance=0, angle=0):
self.distPID = 0
self.anglePID = 0
if (mode == "Angle"):
self.angleController.setSetpoint(angle)
self.distController.disable()
self.angleController.enable()
elif (mode == "Combined"):
self.distController.setSetpoint(distance)
self.angleController.setSetpoint(angle)
self.distController.enable()
self.angleController.enable()
self.prevLeft = 0
self.prevRight = 0
elif (mode == "Direct"):
self.distController.disable()
self.angleController.disable()
self.mode = mode
def setAngle(self, angle):
self.setMode("Angle", angle=angle)
def setCombined(self, distance, angle):
self.setMode("Combined", distance=distance, angle=angle)
def setDirect(self):
self.setMode("Direct")
def sign(self, num):
if (num > 0): return 1
if (num == 0): return 0
return -1
def tankDrive(self, left=0, right=0):
if (self.mode == "Angle"):
nom = self.nominalPIDAngle
if self.anglePID < 0:
nom = -nom
left = self.getMaximum(self.anglePID, nom)
right = self.getMaximum(-self.anglePID, -nom)
elif (self.mode == "Combined"):
nom = self.nominalPID
if self.distPID < 0:
nom = -nom
left = self.getMaximum(self.distPID + self.anglePID, nom)
right = self.getMaximum(self.distPID - self.anglePID, nom)
left = self.getMinimum3(left, 0.30, self.prevLeft + 0.01)
right = self.getMinimum3(right, 0.30, self.prevRight + 0.01)
self.prevLeft = left
self.prevRight = right
elif (self.mode == "Direct"):
[left, right] = [left, right]
else:
[left, right] = [0, 0]
left = min(abs(left), self.maxSpeed) * self.sign(left)
right = min(abs(right), self.maxSpeed) * self.sign(right)
self.__tankDrive__(left, right)
def __tankDrive__(self, left, right):
deadband = 0.1
if (abs(left) > abs(deadband)): self.left.set(left)
else: self.left.set(0)
if (abs(right) > abs(deadband)): self.right.set(right)
else: self.right.set(0)
def getOutputCurrent(self):
return (self.right.getOutputCurrent() +
self.left.getOutputCurrent()) * 3
def updateSensors(self):
if self.navx == None:
self.yaw = 0
self.pitch = 0
self.roll = 0
else:
self.yaw = self.navx.getYaw()
self.pitch = self.navx.getPitch()
self.roll = self.navx.getRoll()
self.leftVal = self.leftEncoder.get()
self.rightVal = self.rightEncoder.get()
def getAngle(self):
angle = self.yaw
if RobotBase.isSimulation(): return -angle
if self.robot.teleop:
if angle == 0:
angle = 0
elif angle < 0:
angle = angle + 180
else:
angle = angle - 180
return angle
def getRoll(self):
return self.roll
def getPitch(self):
return self.pitch
def getRaw(self):
return [self.leftVal, self.rightVal]
def getDistance(self):
return [self.leftVal * self.leftConv, self.rightVal * self.rightConv]
def getAvgDistance(self):
return (self.getDistance()[0] + self.getDistance()[1]) / 2
def getVelocity(self):
dist = self.getDistance()
velocity = [
self.robot.frequency * (dist[0] - self.prevDist[0]),
self.robot.frequency * (dist[1] - self.prevDist[1])
]
self.prevDist = dist
return velocity
def getAvgVelocity(self):
return (self.getVelocity()[0] + self.getVelocity()[1]) / 2
def getAvgAbsVelocity(self):
return (abs(self.getVelocity()[0]) + abs(self.getVelocity()[1])) / 2
def zeroEncoders(self):
self.leftEncoder.reset()
self.rightEncoder.reset()
simComms.resetEncoders()
def zeroNavx(self):
if self.navx == None: pass
else: self.navx.zeroYaw()
def zero(self):
self.zeroEncoders()
self.zeroNavx()
def disable(self):
self.__tankDrive__(0, 0)
def initDefaultCommand(self):
self.setDefaultCommand(SetSpeedDT(timeout=300))
def dashboardInit(self):
SmartDashboard.putData("Turn Angle", TurnAngle())
SmartDashboard.putData("Drive Combined", DriveStraight())
def getMaximum(self, number, comparison):
if math.fabs(number) > math.fabs(comparison): return number
else: return comparison
def getMinimum3(self, val1, val2, val3):
return self.getMinimum(self.getMinimum(val1, val2), val3)
def getMinimum(self, number, comparison):
if math.fabs(number) < math.fabs(comparison): return number
else: return comparison
def isCargoPassed(self):
if self.getAvgDistance() > 16.1: return True
else: return False
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
SmartDashboard.putNumber("Left Counts", self.leftEncoder.get())
SmartDashboard.putNumber("Left Distance",
self.leftEncoder.getDistance())
SmartDashboard.putNumber("Right Counts", self.rightEncoder.get())
#SmartDashboard.putNumber("Right Distance", self.rightEncoder.getDistance())
SmartDashboard.putNumber("DT_DistanceAvg", self.getAvgDistance())
SmartDashboard.putNumber("DT_DistanceLeft", self.getDistance()[0])
SmartDashboard.putNumber("DT_DistanceRight", self.getDistance()[1])
SmartDashboard.putNumber("DT_Angle", self.getAngle())
#SmartDashboard.putNumber("DT_PowerLeft", self.left.get())
#SmartDashboard.putNumber("DT_PowerRight", self.right.get())
#SmartDashboard.putNumber("DT_VelocityLeft", self.getVelocity()[0])
#SmartDashboard.putNumber("DT_VelocityRight", self.getVelocity()[1])
#SmartDashboard.putNumber("DT_CounLeft", self.getRaw()[0])
#SmartDashboard.putNumber("DT_CountRight", self.getRaw()[1])
#SmartDashboard.putNumber("angle correction", self.anglePID)
#SmartDashboard.putNumber("DriveAmps",self.getOutputCurrent())
#self.mode = SmartDashboard.getData("Mode", "Tank")
self.k = SmartDashboard.getNumber("K Value", 1)
self.sensitivity = SmartDashboard.getNumber("sensitivity", 1)
self.driveStyle = SmartDashboard.getString("DriveStyle", "Tank")
class Drive(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Drive')
self.robot = robot
motors = {}
pistons = {}
self.map = self.robot.botMap
self.rEnc = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.lEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X)
self.Gyro = ADXRS450_Gyro()
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
if name == 'Shifter':
pistons[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.dMotors = motors
self.dPistons = pistons
for name in self.dMotors:
self.dMotors[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.dMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.dMotors[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Drive']), 40)
self.kP = 0.0
self.kI = 0.0
self.kD = 0.0
self.DrivePID = PID(self.kP, self.kI, self.kD)
def log(self):
wpilib.SmartDashboard.putNumber('RDrive Enc', self.rEnc.get())
wpilib.SmartDashboard.putNumber('LDrive Enc', self.lEnc.get())
wpilib.SmartDashboard.putNumber('Gyro Val', self.Gyro.getAngle())
def set(self, rgt, lft):
self.dMotors['RFDrive'].set(ctre.ControlMode.PercentOutput, rgt)
self.dMotors['LFDrive'].set(ctre.ControlMode.PercentOutput, lft)
def stop(self):
self.set(0, 0)
def resetGryo(self):
self.Gyro.reset()
def resetEnc(self):
self.rEnc.reset()
self.lEnc.reset()
def setSpeed(self, mode):
self.dPistons['shifter'].set(mode)
def getAngle(self):
x = self.Gyro.getAngle()
if x > 360 or x < -360:
self.Gyro.reset()
return x
def getEnc(self):
l = self.lEnc.get()
r = self.rEnc.get()
return (l + r) / 2
class Drive(Subsystem):
def __init__(self, robot):
super().__init__("Drive")
self.robot = robot
self.peakCurrentLimit = 25
self.PeaKDuration = 50
self.continuousLimit = 15
self.map = robot.RobotMap
# drive motor, sensors, and pistons
motor = {}
pistons = {}
# create all drive motors, Sensors, and pistons
for name in self.map.motorMap.motors:
motor[name] = robot.Creator.createMotor(self.map.motorMap.motors[name])
for name in self.map.PneumaticMap.pistons:
if name == 'Shifter':
pistons[name] = robot.Creator.createPistons(self.map.PneumaticMap.pistons[name])
self.REnc = Encoder(0, 1, True, Encoder.EncodingType.k4X)
self.LEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X)
self.Gyro = ADXRS450_Gyro()
# make motors, Sensors, and pistons local to subsystem
self.Dmotor = motor
self.Dpiston = pistons
for name in self.Dmotor:
self.Dmotor[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.Dmotor[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.Dmotor[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.Dmotor[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.Dmotor[name].enableCurrentLimit(True)
self.KtorqueMode = DoubleSolenoid.Value.kReverse
self.KspeedMode = DoubleSolenoid.Value.kForward
if wpilib.RobotBase.isSimulation():
self.kp = 0.001
self.ki = 0.00001
self.kd = 0.000000001
else:
self.kp = 0.001
self.ki = 0.00001
self.kd = 0.000000001
self.DrivePID = PID(self.kp, self.ki, self.kd)
def Log(self):
wpilib.SmartDashboard.putNumber('rgt enc', self.REnc.get())
wpilib.SmartDashboard.putNumber('lft enc', self.LEnc.get())
wpilib.SmartDashboard.putNumber('drive rgt frt', self.Dmotor['RFDrive'].getOutputCurrent())
wpilib.SmartDashboard.putNumber('drive lft frt', self.Dmotor['LFDrive'].getOutputCurrent())
"""
Get Functions
"""
def getEncoderVal(self):
# get encoder values from both sides
rgt = self.REnc.get()
lft = self.LEnc.get()
avg = (rgt+lft)/2 # find average
return avg
def getGyroVal(self):
# get gyro values
x = self.Gyro.getAngle()
# reset gyro if it passes 360 deg
if x > 360 or x < -360:
self.resetGyro()
return self.Gyro.getAngle()
"""
Set Functions
"""
def set(self, rgt, lft):
self.Dmotor['RFDrive'].set(ctre.ControlMode.PercentOutput, rgt)
self.Dmotor['LFDrive'].set(ctre.ControlMode.PercentOutput, lft)
# uncomment when all motors are master
"""self.Dmotor['RMDrive'].set(self.Dmotor['RMDrive'].ControlMode.PercentOutput, rgt)
self.Dmotor['RBDrive'].set(self.Dmotor['RBDrive'].ControlMode.PercentOutput, rgt)"""
"""self.Dmotor['LMDrive'].set(self.Dmotor['LMDrive'].ControlMode.PercentOutput, lft)
self.Dmotor['LBDrive'].set(self.Dmotor['LBDrive'].ControlMode.PercentOutput, lft)"""
def stop(self):
self.set(0, 0)
def ShiftGear(self, GearMode):
self.Dpiston['Shifter'].set(GearMode)
def resetEnc(self):
self.REnc.reset()
self.LEnc.reset()
def resetGyro(self):
self.Gyro.reset()
