Add CTRE position_closed_loop example

examples/position_closed_loop/physics.py

@@ -0,0 +1,29 @@
+
+class PhysicsEngine(object):
+    '''
+        Simulates a single motor connected to an encoder to make the
+        motion magic example work correctly
+    '''
+
+    def __init__(self, physics_controller):
+        '''
+            :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object
+                                       to communicate simulation effects to
+        '''
+
+        self.physics_controller = physics_controller
+        self.position = 0
+
+    def update_sim(self, hal_data, now, tm_diff):
+
+        try:
+            talon_data = hal_data['CAN'][3]
+        except (KeyError, IndexError):
+            # talon must not be initialized yet
+            return
+
+        # encoder increments speed mutiplied by the time by some constant
+        # -> must be an integer
+        speed = int(4096*4 * talon_data['value'] * tm_diff)
+        talon_data['quad_position'] += speed
+        talon_data['quad_velocity'] = speed

examples/position_closed_loop/robot.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+
+'''
+ * Example demonstrating the Position closed-loop servo.
+ * Tested with Logitech F350 USB Gamepad inserted into Driver Station]
+ *
+ * Be sure to select the correct feedback sensor using configSelectedFeedbackSensor() below.
+ *
+ * After deploying/debugging this to your RIO, first use the left Y-stick
+ * to throttle the Talon manually.  This will confirm your hardware setup.
+ * Be sure to confirm that when the Talon is driving forward (green) the
+ * position sensor is moving in a positive direction.  If this is not the cause
+ * flip the boolean input to the setSensorPhase() call below.
+ *
+ * Once you've ensured your feedback device is in-phase with the motor,
+ * use the button shortcuts to servo to target position.
+ *
+ * Tweak the PID gains accordingly.
+'''
+
+from ctre import WPI_TalonSRX
+import wpilib
+
+
+class Robot(wpilib.IterativeRobot):
+
+    #: Which PID slot to pull gains from. Starting 2018, you can choose from
+    #: 0,1,2 or 3. Only the first two (0,1) are visible in web-based
+    #: configuration.
+    kSlotIdx = 0
+
+    #: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For
+    #: now we just want the primary one.
+    kPIDLoopIdx = 0
+
+    #: set to zero to skip waiting for confirmation, set to nonzero to wait and
+    #: report to DS if action fails.
+    kTimeoutMs = 10
+
+
+    def robotInit(self):
+        self.talon = WPI_TalonSRX(3)
+        self.joy = wpilib.Joystick(0)
+
+        self.loops = 0
+        self.lastButton1 = False
+        self.targetPos = 0
+
+        # choose the sensor and sensor direction
+        self.talon.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs)
+
+        # choose to ensure sensor is positive when output is positive
+        self.talon.setSensorPhase(True)
+
+        # choose based on what direction you want forward/positive to be.
+        # This does not affect sensor phase.
+        self.talon.setInverted(False)
+
+        # set the peak and nominal outputs, 12V means full
+        self.talon.configNominalOutputForward(0, self.kTimeoutMs)
+        self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
+        self.talon.configPeakOutputForward(1, self.kTimeoutMs)
+        self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
+
+        # Set the allowable closed-loop error, Closed-Loop output will be
+        # neutral within this range. See Table in Section 17.2.1 for native
+        # units per rotation.
+        self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx, self.kTimeoutMs)
+
+        # set closed loop gains in slot0, typically kF stays zero - see documentation */
+        self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
+        self.talon.config_kF(0, 0, self.kTimeoutMs)
+        self.talon.config_kP(0, 0.1, self.kTimeoutMs)
+        self.talon.config_kI(0, 0, self.kTimeoutMs)
+        self.talon.config_kD(0, 0, self.kTimeoutMs)
+
+        # zero the sensor
+        self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
+
+    def teleopPeriodic(self):
+        '''
+            This function is called periodically during operator control
+        '''
+        # get gamepad axis - forward stick is positive
+        leftYstick = self.joy.getY()
+        # calculate the percent motor output
+        motorOutput = self.talon.getMotorOutputPercent()
+        button1 = self.joy.getRawButton(1)
+        button2 = self.joy.getRawButton(2)
+
+        # deadband gamepad
+        if abs(leftYstick) < 0.1:
+            leftYstick = 0
+
+        # prepare line to print
+        sb = []
+        sb.append("\tOut%%: %.3f" % motorOutput)
+        sb.append("\tPos: %.3fu" % self.talon.getSelectedSensorPosition(self.kPIDLoopIdx))
+
+        if self.lastButton1 and button1:
+            # Position mode - button just pressed
+
+            # 10 Rotations * 4096 u/rev in either direction
+            self.targetPos = leftYstick * 4096 * 10.0
+            self.talon.set(WPI_TalonSRX.ControlMode.Position, self.targetPos)
+
+        # on button2 just straight drive
+        if button2:
+            # Percent voltage mode
+            self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
+
+        if self.talon.getControlMode() == WPI_TalonSRX.ControlMode.Position:
+            # append more signals to print when in speed mode.
+            sb.append("\terr: %s" % self.talon.getClosedLoopError(self.kPIDLoopIdx))
+            sb.append("\ttrg: %.3f" % self.targetPos)
+
+        # periodically print to console
+        self.loops += 1
+        if self.loops >= 10:
+            self.loops = 0
+            print(' '.join(sb))
+
+        # save button state for on press detect
+        self.lastButton1 = button1
+
+if __name__ == '__main__':
+    wpilib.run(Robot)