Initial Commit

claw.py

@@ -0,0 +1,103 @@
+import wpilib
+import rev
+from logger import Logger
+
+from robotconfig import MODULE_NAMES
+
+DASH_PREFIX = MODULE_NAMES.CLAW
+
+class Claw:
+
+    def __init__(self, motor_id, _cone_default_release_speed, _cone_upper_scoring_height_release_speed, _cone_lower_scoring_height_release_speed, _cube_default_release_speed, _cube_upper_scoring_height_release_speed, _cube_lower_scoring_height_release_speed, _release_change, _intake_speed, _intake_change):
+        self.logger = Logger.getLogger()
+        motor_type = rev.CANSparkMaxLowLevel.MotorType.kBrushless
+        self.motor = rev.CANSparkMax(motor_id, motor_type)
+        self.encoder = self.motor.getEncoder()
+
+        # Just to make sure it's defined and off.
+        self.basePosition = self.encoder.getPosition()
+        self.motor.set(0)
+
+        self.coneDefaultReleaseSpeed = _cone_default_release_speed
+        self.coneUpperReleaseSpeed = _cone_upper_scoring_height_release_speed
+        self.coneLowerReleaseSpeed = _cone_lower_scoring_height_release_speed
+        self.cubeDefaultReleaseSpeed = _cube_default_release_speed
+        self.cubeUpperReleaseSpeed = _cube_upper_scoring_height_release_speed
+        self.cubeLowerReleaseSpeed = _cube_lower_scoring_height_release_speed
+
+        self.motor.setOpenLoopRampRate(0.0)
+
+        self.releaseSpeed = self.coneDefaultReleaseSpeed
+        self.releaseChange = _release_change
+        self.intakeSpeed = _intake_speed
+        self.intakeChange = _intake_change
+
+        # Resets maneuver. Note that this is also reset with off(), which is called in roboty.py: disabledExit()
+        self.maneuverComplete = True
+
+    def setConeSpeedDefault(self):
+        self.releaseSpeed = self.coneDefaultReleaseSpeed
+
+    def setConeSpeedUpper(self):
+        self.releaseSpeed = self.coneUpperReleaseSpeed
+
+    def setConeSpeedLower(self):
+        self.releaseSpeed = self.coneLowerReleaseSpeed
+
+    def setCubeSpeedDefault(self):
+        self.releaseSpeed = self.cubeDefaultReleaseSpeed
+
+    def setCubeSpeedUpper(self):
+        self.releaseSpeed = self.cubeUpperReleaseSpeed
+
+    def setCubeSpeedLower(self):
+        self.releaseSpeed = self.cubeLowerReleaseSpeed
+
+    def getReleaseSpeed(self):
+        return self.releaseSpeed
+
+    # Expel the object by running motors to expel.
+    def release(self):
+        self.motor.set(self.releaseSpeed)
+
+    # Expel the object by running motors to expel.
+    def intake(self):
+        self.motor.set(-self.intakeSpeed)
+
+    # Stop the claw motor.
+    def off(self):
+        self.motor.set(0)
+        self.maneuverComplete = True
+
+    def runAndStop(self, direction):
+
+        # First time in, so figure out where we are and how far to go.
+        if self.maneuverComplete == True:
+            self.basePosition = self.encoder.getPosition()
+            if direction >= 0:
+                self.targetPosition = self.basePosition + self.releaseChange
+            else:
+                self.targetPosition = self.basePosition - self.intakeChange
+            self.maneuverComplete = False
+            self.log("Claw: RunNStop: FirstTime: basePosition: ", self.basePosition, " self.targetPosition", self.targetPosition)
+
+        # Are we expelling game piece and not there yet?
+        if (self.maneuverComplete == False) and (self.basePosition < self.targetPosition) and (self.encoder.getPosition() < self.targetPosition):
+            self.motor.set(self.releaseSpeed)
+            self.log("Claw: RunNStop: Expelling / Not Done: basePosition: ", self.basePosition, " self.targetPosition", self.targetPosition, " getPosition: ", self.encoder.getPosition())
+            return False
+
+        # Are we grabbing game piece and not there yet?
+        elif (self.maneuverComplete == False) and (self.basePosition > self.targetPosition) and (self.encoder.getPosition() > self.targetPosition):
+            self.motor.set(-self.intakeSpeed)
+            self.log("Claw: RunNStop: Intake / Not Done: basePosition: ", self.basePosition, " self.targetPosition", self.targetPosition, " getPosition: ", self.encoder.getPosition())
+            return False
+
+        # We must be done, so end maneuver.
+        else:
+            self.motor.set(0)
+            self.maneuverComplete = True
+            return True
+
+    def log(self, *dataToLog):
+        self.logger.log(DASH_PREFIX, dataToLog)

cliffdetector.py

@@ -0,0 +1,26 @@
+import wpilib
+
+class CliffDetector:
+    def __init__(self, _leftCliffDetectorPingID, 
+    _leftCliffDetectorEchoID, _rightCliffDetectorPingID, 
+    _rightCliffDetectorEchoID, _cliffTolerance):
+        self.cliffTolerance = _cliffTolerance
+        self.Lsensor = wpilib.Ultrasonic(_leftCliffDetectorPingID,_leftCliffDetectorEchoID)
+        self.Rsensor = wpilib.Ultrasonic(_rightCliffDetectorPingID, _rightCliffDetectorEchoID)
+        self.Lsensor.setAutomaticMode(True)
+        self.Rsensor.setAutomaticMode(True)
+
+    def atCliff(self):
+        leftDistance = self.Lsensor.getRange()
+        rightDistance = self.Rsensor.getRange()
+        difference = leftDistance - rightDistance
+        if difference > self.cliffTolerance:
+            return -1 # at Left Cliff
+        if difference < -self.cliffTolerance:
+            return 1 # at Right Cliff
+        return 0 # not at Cliff
+
+    def update(self):
+        print(self.Lsensor.getRange())
+        print(self.Rsensor.getRange())
+

controller.py

@@ -0,0 +1,7 @@
+class Controller:
+    def __init__(self, xboxController: None, deadzone: 0.0,
+                 left_trigger_axis: 2, right_trigger_axis: 3):
+        self.xboxController = xboxController
+        self.deadzone = deadzone
+        self.left_trigger_axis = left_trigger_axis
+        self.right_trigger_axis = right_trigger_axis

dashboard.py

@@ -0,0 +1,44 @@
+
+from networktables import NetworkTables
+from robotconfig import dashboardConfig, MODULE_NAMES
+
+class Dashboard:
+
+    theDashboard = None
+
+    def __init__(self, testMode):
+        self.tableName = 'SmartDashboard'
+        self.server = 'roborio-1076-frc.local'
+        if testMode:
+            self.server = 'localhost'
+        NetworkTables.initialize(server=self.server) # Necessary for vision to
+        self.dashboard = NetworkTables.getTable('SmartDashboard')
+
+    def getTable(self):
+        return self.dashboard
+
+    def putNumber(self, moduleName, key, value):
+        if dashboardConfig[moduleName]:
+            self.dashboard.putNumber(moduleName + '/' + key, value)
+
+    def putBoolean(self, moduleName, key, value):
+        if dashboardConfig[moduleName]:
+            self.dashboard.putBoolean(moduleName + '/' + key, value)
+
+    def putString(self, moduleName, key, value):
+        if dashboardConfig[moduleName]:
+            self.dashboard.putString(moduleName + '/' + key, value)
+
+    def getNumber(self, moduleName, key, defaultValue = -1):
+        return self.dashboard.getNumber(moduleName + '/' + key, defaultValue)
+
+    def getBoolean(self, moduleName, key, defaultValue = False):
+        return self.dashboard.getBoolean(moduleName + '/' + key, defaultValue)
+
+    def getString(self, moduleName, key, defaultValue = ''):
+        return self.dashboard.getBoolean(moduleName + '/' + key, defaultValue)
+
+    def getDashboard(testMode=False):
+        if not Dashboard.theDashboard:
+            Dashboard.theDashboard = Dashboard(testMode)
+        return Dashboard.theDashboard

elevator.py

@@ -0,0 +1,196 @@
+import rev
+import wpilib
+from wpilib import DoubleSolenoid
+import wpimath.controller
+from wpimath.controller import PIDController
+import math
+
+from logger import Logger
+from robotconfig import MODULE_NAMES
+
+DASH_PREFIX = MODULE_NAMES.ELEVATOR
+
+class Elevator:
+    def __init__(self, right_id, left_id, solenoid_forward_id, solenoid_reverse_id, kP, kI, kD, lower_safety, upper_safety, grabber, left_limit_switch_id, right_limit_switch_id):
+        self.logger = Logger.getLogger()
+        motor_type = rev.CANSparkMaxLowLevel.MotorType.kBrushless
+        self.right_motor = rev.CANSparkMax(right_id, motor_type) # elevator up-down
+        self.left_motor = rev.CANSparkMax(left_id, motor_type) # elevator up-down
+        self.right_encoder = self.right_motor.getEncoder() # measure elevator height
+        self.left_encoder = self.left_motor.getEncoder() # ""
+        self.right_encoder.setPosition(0)
+        self.left_encoder.setPosition(0)
+        self.solenoid = wpilib.DoubleSolenoid(1, # controls the "lean" of the elevator
+            wpilib.PneumaticsModuleType.REVPH, 
+            solenoid_forward_id, 
+            solenoid_reverse_id)
+        self.pid_controller = PIDController(kP, kI, kD)
+        self.pid_controller.setTolerance(0.3, 0.01)
+        self.grabber = grabber
+        self.right_motor.setOpenLoopRampRate(0.50)
+        self.left_motor.setOpenLoopRampRate(0.50)
+        self.upperSafety = upper_safety
+        self.lowerSafety = lower_safety
+        self.left_limit_switch = wpilib.DigitalInput(left_limit_switch_id)
+        self.right_limit_switch = wpilib.DigitalInput(right_limit_switch_id)
+        self.targetPosition = self.getEncoderPosition()
+
+        self.storeElevatorBypassLimitSwitch = False
+        self.elevatorHasReset = False
+
+        self.allowToggle = True
+        self.cycleCounter = 0
+
+    def resetElevator(self):
+        self.storeElevatorBypassLimitSwitch = False
+        self.elevatorHasReset = False
+
+    def hasElevatorReset(self):
+        return self.elevatorHasReset
+
+    def getTargetPosition(self):
+        return self.targetPosition
+
+    #1.00917431193 inches per rotation
+    def extend(self, targetSpeed):  # controls length of the elevator 
+
+        if targetSpeed > 1:
+            targetSpeed = 1
+        if targetSpeed < -1:
+            targetSpeed = -1
+
+        if targetSpeed > 0:
+            targetSpeed *= 0.5
+
+        self.log("Elevator: Extend: getEncoderPosition: ", self.getEncoderPosition(), " targetSpeed: ", targetSpeed)
+
+        #make sure arm doesn't go past limit
+        if self.getEncoderPosition() > self.upperSafety and targetSpeed < 0:
+            self.right_motor.set(0)
+            self.left_motor.set(0)
+            return
+        if self.getEncoderPosition() < self.lowerSafety and targetSpeed > 0:
+            self.right_motor.set(0)
+            self.left_motor.set(0)
+            return
+
+        self.right_motor.set(-targetSpeed)
+        self.left_motor.set(-targetSpeed)
+        #self.left_motor.set(0)
+
+    def motors_off(self):
+        self.right_motor.set(0)
+        self.left_motor.set(0)
+
+    # Move elevator and reset target to where you end up.
+    def move(self, targetSpeed):
+        self.extend(targetSpeed)
+        self.targetPosition = self.getEncoderPosition()
+
+    #automatically move to an elevator extension (position) using a pid controller
+    def moveToPos(self, _targetPosition):
+
+        self.targetPosition = _targetPosition
+        extendSpeed = self.pid_controller.calculate(self.getEncoderPosition(), self.targetPosition)
+        self.log("Elevator: moveToPos: ", self.pid_controller.getSetpoint(), " actual position: ", self.getEncoderPosition())
+        if(self.pid_controller.atSetpoint()):
+        #if(self.nearSetpoint(2)):
+            self.log("Elevator: At set point", self.getEncoderPosition())
+            self.extend(0)
+            return True
+        else:
+            self.log("Elevator: Moving")
+            extendSpeed *= -1 # Elevator motor moves reverse direction.
+            self.extend(extendSpeed * 0.1125)
+            if (self.nearSetpoint(3)):
+                return True
+            return False
+
+    def nearSetpoint(self, range):
+        currentSetpoint = abs(self.pid_controller.getSetpoint())
+        currentPosition = abs(self.getEncoderPosition())
+        diff = abs(currentPosition - currentSetpoint)
+
+        self.log("Elevator: moveToPos: SP: ", currentSetpoint, " pos: ", currentPosition, " diff: ", diff)
+
+        if diff < range:
+            return True
+        return False 
+
+    def update(self):
+        self.moveToPos(self.targetPosition)
+        self.log("Elevator: Update: Left Limit: ", self.left_limit_switch.get(), " Right Limit: ", self.right_limit_switch.get())
+
+    def isElevatorDown(self):
+        if self.solenoid.get() == DoubleSolenoid.Value.kForward or self.solenoid.get() == DoubleSolenoid.Value.kOff:
+            return True
+        return False
+
+    def isElevatorUp(self):
+        if self.solenoid.get() == DoubleSolenoid.Value.kReverse:
+            return True
+        return False
+
+    def elevatorUp(self):
+        self.solenoid.set(DoubleSolenoid.Value.kForward)
+        return True
+
+    def elevatorDown(self):
+        self.solenoid.set(DoubleSolenoid.Value.kReverse)
+        return True
+
+    # contols the "lean" of the elevator
+    def toggle(self):
+        self.log("Elevator: In toggle().")
+
+        if (self.allowToggle == False) and (self.cycleCounter % 100 == 0):
+            self.allowToggle = True
+            self.cycleCounter = 0
+            self.log("Elevator: Toggle: Resetting cycleCOunter and allowing Toggle.")
+        if self.allowToggle == False:
+            self.log("Elevator: Toggle: Incrementing cycleCounter.")
+            self.cycleCounter += 1
+            return True
+        if self.solenoid.get() == DoubleSolenoid.Value.kForward:
+            self.solenoid.set(DoubleSolenoid.Value.kReverse)
+            self.log("Elevator: Toggle: Set to reverse/up.")
+            self.cycleCounter = 0
+            self.allowToggle = False
+        elif self.solenoid.get() == DoubleSolenoid.Value.kReverse or self.solenoid.get() == DoubleSolenoid.Value.kOff:
+            self.solenoid.set(DoubleSolenoid.Value.kForward)
+            self.log("Elevator: Toggle: Set forward/down.")
+            self.cycleCounter = 0
+            self.allowToggle = False
+        else:
+            self.log("Elevator: Toggle: How did we get here?")
+        return True
+
+    def resetEncoders(self):
+        self.left_encoder.setPosition(0)
+        self.right_encoder.setPosition(0)
+        self.targetPosition = self.getEncoderPosition()
+
+    def bypassLimitSwitch(self):
+        self.log("Elevator: Bypassing limit switch reset.")
+        self.storeElevatorBypassLimitSwitch = True
+
+    def elevatorReset(self):
+        self.log("Elevator: Reseting elevator: Left Limit: ", self.left_limit_switch.get(), " Right Limit: ", self.right_limit_switch.get())
+
+        if self.left_limit_switch.get() or self.right_limit_switch.get() or self.storeElevatorBypassLimitSwitch:
+            self.log("Elevator: Found the limit switch")
+            self.resetEncoders()
+            self.elevatorHasReset = True
+            return True
+        else:
+            self.right_motor.set(0.1)
+            self.left_motor.set(0.1)
+            self.elevatorHasReset = False
+            return False
+
+    # only reading the right encoder, assuming that left and right will stay about the same
+    def getEncoderPosition(self):
+        return (self.right_encoder.getPosition() + self.left_encoder.getPosition()) / 2
+
+    def log(self, *dataToLog):
+        self.logger.log(DASH_PREFIX, dataToLog)