Merge pull request #73 from qubit-robotics/potantiometerpid

add potentiometerpid example

Author: virtuald

potentiometer-pid/robot.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath.controller
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a sample program to demonstrate how to use a soft potentiometer and
+    a PID controller to reach and maintain position setpoints on an elevator
+    mechanism.
+    """
+
+    kPotChannel = 1
+    kMotorChannel = 7
+    kJoystickChannel = 3
+
+    # The elevator can move 1.5 meters from top to bottom
+    kFullHeightMeters = 1.5
+
+    # Bottom, middle, and top elevator setpoints
+    kSetpointMeters = [0.2, 0.8, 1.4]
+
+    # proportional, integral, and derivative speed constants
+    # DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
+    # and kD may cause dangerous, uncontrollable, or undesired behavior!
+    kP = 0.7
+    kI = 0.35
+    kD = 0.25
+
+    def robotInit(self):
+        self.pidController = wpimath.controller.PIDController(self.kP, self.kI, self.kD)
+        # Scaling is handled internally
+        self.potentiometer = wpilib.AnalogPotentiometer(
+            self.kPotChannel, self.kFullHeightMeters
+        )
+        self.elevatorMotor = wpilib.PWMSparkMax(self.kMotorChannel)
+        self.joystick = wpilib.Joystick(self.kJoystickChannel)
+
+    def teleopInit(self):
+        # Move to the bottom setpoint when teleop starts
+        self.index = 0
+        self.pidController.setSetpoint(self.kSetpointMeters[self.index])
+
+    def teleopPeriodic(self):
+        # Read from the sensor
+        position = self.potentiometer.get()
+
+        # Run the PID Controller
+        pidOut = self.pidController.calculate(position)
+
+        # Apply PID output
+        self.elevatorMotor.set(pidOut)
+
+        # when the button is pressed once, the selected elevator setpoint is incremented
+        if self.joystick.getTriggerPressed():
+            # index of the elevator setpoint wraps around.
+            self.index = (self.index + 1) % len(self.kSetpointMeters)
+            print(f"m_index = {self.index}")
+            self.pidController.setSetpoint(self.kSetpointMeters[self.index])
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

run_tests.sh

@@ -26,6 +26,7 @@ BASE_TESTS="
   physics-4wheel/src
   physics-mecanum/src
   physics-spi/src
+  potentiometer-pid
   relay
   shuffleboard
   stateful-autonomous