Auton and PID Abstraction

robot/autonomous/auto.py

@@ -0,0 +1,104 @@
+# Non-Magnolia Autonomous
+import networktables
+import rev
+from components.boom import Boom
+from components.drivetrain import DriveTrain
+from components.encoders import Encoder
+from components.grabber import Grabber
+from components.gyro import Gyro
+from controllers.boom_controller import BoomController
+from ctre import NeutralMode
+from magicbot import AutonomousStateMachine, state, timed_state
+import navx
+
+BRAKE_MODE = NeutralMode(2)
+COAST_MODE = NeutralMode(1)
+# ARM_ENCODER = 
+
+class Autonomous(AutonomousStateMachine):
+    DEFAULT = True
+    MODE_NAME = "auton"
+    sd: networktables.NetworkTable
+    drivetrain: DriveTrain
+    boom_arm: Boom
+    grabber: Grabber
+    gyro: Gyro
+    encoder: Encoder
+    boom_extender_motor_encoder: rev.SparkMaxRelativeEncoder
+    boom_controller: BoomController
+
+
+    @state(first = True)
+    def initial(self):
+            # Gyro is initialized/calibrated
+        self.navx = navx.AHRS.create_spi()
+        self.gyro.reset()
+
+        '''NOTE: ENCODER POSITIONS NEED TO BE INITIALIZED'''
+        self.boom_extender_motor_encoder.setPosition(0)
+            # For the Drivetrain
+        self.drivetrain.drivetrain_encoder_right.setPosition(0)
+        self.drivetrain.drivetrain_encoder_left.setPosition(0)
+            # For the Rotator
+        self.boom_arm.boom_rotator_motor1.setSelectedSensorPosition(0) # Memphis Pyramid no lai
+        self.boom_arm.boom_rotator_motor2.setSelectedSensorPosition(0)
+        self.sd.putValue("Auton_Status: ", "Starting")
+        self.next_state("Raise1")
+
+    ''' NOTE: TO SELF: ROT_AVG and settarget are USING PLACEHOLDERS, MAKE SURE TO TEST FOR VALUES LATER!!!! '''
+
+    @state
+    def Raise1(self):
+        ROT_AVG = (self.boom_arm.boom_rotator_motor1.getSelectedSensorPosition() + 
+                   self.boom_arm.boom_rotator_motor2.getSelectedSensorPosition()) / 2
+        if ROT_AVG != 0:
+            self.boom_controller.set_target(1000)
+            self.sd.putValue("Auton_Status: ", "Initial Rotate")
+        else:
+            self.boom_controller.set_target(0)
+            self.sd.putValue("Auton_Status: ", "SUCCESS!")
+            self.next_state("grab1")
+
+    @state
+    def grab1(self):
+        self.grabber.solenoid_toggle()
+        self.sd.putValue("Auton_Status: ", "Grabbing")
+        self.next_state("Raise2")
+
+    @state
+    def Raise2(self):
+        ROT_AVG2 = (self.boom_arm.boom_rotator_motor1.getSelectedSensorPosition() + 
+                   self.boom_arm.boom_rotator_motor2.getSelectedSensorPosition()) / 2
+        if ROT_AVG2 != 0:
+            self.boom_controller.set_target(0)
+            self.sd.putValue("Auton_Status: ", "Raising Arm")
+        else:
+            self.boom_controller.set_target(0)
+            self.sd.putValue("Auton_Status: ", "Success!")
+            self.next_state("drop1")
+            # NOTE: These statements are a bit janky, but its a redundancy
+
+    @state
+    def drop1(self):
+        self.grabber.solenoid_toggle()
+        self.sd.putValue("Auton_Status: ", "Dropping.")
+        self.next_state("retract")
+
+    @state 
+    def retract(self):
+        self.boom_controller.set_target(2000) # Again, placeholder
+        self.grabber.solenoid_toggle() # This prevents grabber damage 
+        self.sd.putValue("Auton_Status: ", "Retracting")
+        self.next_state("driveback_1")
+
+    @timed_state(duration = 2, next_state = "done")
+    def driveback_1(self):
+        self.drivetrain.set_motors(-0.6, 0.0)
+        self.sd.putValue("Auton_Status: ", "Driving Back")
+
+    @state
+    def done(self):
+        self.drivetrain.set_motors(0.0, 0.0)
+        self.sd.putValue("Auton_Status: ", "Auton Done")
+
+

robot/autonomous/basicauto.py → robot/autonomous/timedauto.py

@@ -13,8 +13,8 @@
 BRAKE_MODE = NeutralMode(2)
 COAST_MODE = NeutralMode(1)
 class Autonomous(AutonomousStateMachine):
-    DEFAULT = True
-    MODE_NAME = "autodrive"
+    DEFAULT = False
+    MODE_NAME = "timed_state_auto"
     sd: networktables.NetworkTable
     drivetrain: DriveTrain
     boom_arm: Boom

robot/components/gyro.py

@@ -6,12 +6,21 @@
 import networktables
 from networktables import NetworkTable
 from magicbot import MagicRobot
+from ctre import WPI_TalonFX
+from ctre import NeutralMode
 # Various imports
 
+BRAKE_MODE = NeutralMode(2)
+COAST_MODE = NeutralMode(1)
+
 class Gyro():
     # MagicBot Variable Injection
     sd: networktables.NetworkTable
     drivetrain: DriveTrain
+    talon_L_1: WPI_TalonFX
+    talon_L_2: WPI_TalonFX
+    talon_R_1: WPI_TalonFX
+    talon_R_2: WPI_TalonFX
 
     def setup(self):
         self.navx = navx.AHRS.create_spi()
@@ -27,45 +36,51 @@ def balancing(self):
         ''' Just making things a bit easier to read here with variables'''
         # This should provide constant adjustment to the speeds so it won't jerk the robot
 
-        print(f'ANGLE_RETURN: {angle}')
+        self.sd.putValue("ANGLE_RETURN: ", angle)
+        # Puts angle return in SD
 
         try:
         # try and except 
             if (angle < MAX_RANGE_LIM_HI) and (angle > MAX_RANGE_LIM_LOW):
                 # PRECHECK TO MAKE SURE GYRO IS IN GOOD CONDITION
+
                 if (angle < DEFAULT_LOW) and (angle > MAX_RANGE_LIM_LOW):
+
                     self.drivetrain.set_motors(0.33, 0.0)
-                    self.sd.putValue("Mode: ", "Moving Forward")
-                    print("MODE: ST1")
+                    self.sd.putValue("gyro_status: ", "Moving Forward")
                     # Forward
 
                 elif (angle > DEFAULT_HI) and (angle < MAX_RANGE_LIM_HI):
+
                     self.drivetrain.set_motors(-0.33, 0.0)
-                    self.sd.putValue("Mode: ", "Moving Backward")
-                    print("MODE: ST2")
+                    self.sd.putValue("gyro_status: ", "Moving Backward")
                     # Backward
 
                 elif (angle < DEFAULT_HI) and (angle > DEFAULT_LOW):
+
                     self.drivetrain.set_motors(0.0, 0.0)
-                    self.sd.putValue("Mode: ", "Balanced!")
-                    print("balanced!")
+                    self.sd.putValue("gyro_status: ", "Balanced!")
                     print(self.navx.getPitch())
+                    # Braking after balancing :)
+                    self.talon_L_1.setNeutralMode(BRAKE_MODE)
+                    self.talon_L_2.setNeutralMode(BRAKE_MODE)
+                    self.talon_R_1.setNeutralMode(BRAKE_MODE)
+                    self.talon_R_2.setNeutralMode(BRAKE_MODE)
                     # Balanced
             else:
                 # In this case: Either bot is flipped, or error values
                 print("ERROR: OUT OF BOUNDS: E.1")
                 self.drivetrain.set_motors(0.0, 0.0)
-                self.sd.putValue("Mode: ", "GYRO ERROR")
+                self.sd.putValue("gyro_status: ", "GYRO ERROR")
         except:
             print("ERROR: EXTERNAL ISSUE: E.2")
 
     # Try and except statement in case something goes bad, this is an effort to protect motors and the robot during autonomous
 
     def reset(self):
         self.navx.reset()
-        self.sd.putValue("MODE: ", "navx_reset")
-        print("STATE: RESETTING NAVX")
-
+        self.sd.putValue("gyro_status: ", "navx_reset")
+
     def execute(self):
         # just passes through this to execute gyro in robot.py
         pass

robot/controllers/boom_controller.py

@@ -0,0 +1,59 @@
+'''PID controller for arm rotations'''
+# package imports
+import rev
+from wpimath.controller import PIDController
+
+# component imports
+from components.boom import Boom
+
+PID_TARGET_INPUT_MULTIPLIER = 1000
+PID_FRONT_TARGET_LIMIT = -90000
+
+class BoomController():
+
+    # magicbot is cool like that
+    boom_arm: Boom
+    boom_extender_motor_encoder: rev.SparkMaxRelativeEncoder
+
+    def setup(self):
+        self.armPID = PIDController(0.00002, 0.00004, 0.000001, 0.02)
+        self.armPID.setTolerance(50)
+        self.pidOutput = 0
+        self.pidEnabled = False
+        self.boom_extender_motor_encoder.setPosition(0)
+        self.boom_arm.boom_rotator_motor1.setSelectedSensorPosition(0)
+        self.boom_arm.boom_rotator_motor2.setSelectedSensorPosition(0)
+        self.armPID.reset()
+        self.pidTarget = -3000
+
+    def toggle_pid(self) -> None:
+
+        # Toggles the PID state(on/off)
+        self.pidEnabled = not self.pidEnabled
+        if self.pidEnabled:
+            self.pidTarget = (
+                 self.boom_arm.boom_rotator_motor1.getSelectedSensorPosition() +
+                   self.boom_arm.boom_rotator_motor2.getSelectedSensorPosition()) / 2
+            self.armPID.setSetpoint(self.pidTarget)
+            self.pidEnabled = True
+            self.armPID.reset()
+
+    def set_target(self, pidTarget) -> None:
+
+        # Sets the PID Value, see robot.py
+        if (self.pidTarget < PID_FRONT_TARGET_LIMIT):
+            self.pidTarget = PID_FRONT_TARGET_LIMIT
+        if (self.pidTarget > -1100):
+            self.pidTarget = -1100
+        self.armPID.setSetpoint(pidTarget)
+        if not self.pidEnabled:
+            pidTarget = (
+                self.boom_arm.boom_rotator_motor1.getSelectedSensorPosition() +
+                  self.boom_arm.boom_rotator_motor2.getSelectedSensorPosition()) / 2
+            self.armPID.setSetpoint(pidTarget)
+            self.pidEnabled = True
+            self.armPID.reset()
+
+    def execute(self):
+        pass
+