Support follower mode (#12)

* Fix built in PID calculation

* Add follower mode

Author: ThadHouse

expansionhub/MotorNtState.cpp

@@ -6,6 +6,7 @@
 #define VOLTAGE_MODE 1
 #define POSITION_PID_MODE 2
 #define VELOCITY_PID_MODE 3
+#define FOLLOWER_MODE 4
 
 using namespace eh;
 
@@ -23,28 +24,31 @@ void MotorNtState::SetEncoder(double positionRaw, double velocityRaw) {
     velocityPublisher.Set(lastEncoderVelocity);
 }
 
-double MotorNtState::ComputeMotorPower(double batteryVoltage) {
+std::pair<double, int> MotorNtState::ComputeMotorPower(double batteryVoltage) {
     double reversed = reversedSubscriber.Get(false) ? -1.0 : 1.0;
     if (batteryVoltage == 0) {
-        return 0;
+        return {0.0, -1};
     }
     double setpoint = setpointSubscriber.Get(0);
     switch (modeSubscriber.Get(PERCENTAGE_MODE)) {
         case VOLTAGE_MODE:
-            return (setpoint / batteryVoltage) * reversed;
+            return {(setpoint / batteryVoltage) * reversed, -1};
 
         case POSITION_PID_MODE:
-            return (positionPid.Compute(setpoint, lastEncoderPosition) /
+            return {(positionPid.Compute(setpoint, lastEncoderPosition) /
                     batteryVoltage) *
-                   reversed;
+                   reversed, -1};
 
         case VELOCITY_PID_MODE:
-            return (velocityPid.Compute(setpoint, lastEncoderVelocity) /
+            return {(velocityPid.Compute(setpoint, lastEncoderVelocity) /
                     batteryVoltage) *
-                   reversed;
+                   reversed, -1};
+
+        case FOLLOWER_MODE:
+            return {0.0, static_cast<int>(setpoint)};
 
         default:
-            return setpoint * reversed;
+            return {setpoint * reversed, -1};
     }
 }
 

expansionhub/MotorNtState.h

@@ -5,6 +5,7 @@
 #include "networktables/DoubleTopic.h"
 #include "PidConstants.h"
 #include "CachedCommand.h"
+#include <utility>
 
 namespace eh {
 
@@ -37,7 +38,7 @@ struct MotorNtState {
     double lastEncoderPosition{0};
     double lastEncoderVelocity{0};
 
-    double ComputeMotorPower(double batteryVoltage);
+    std::pair<double, int> ComputeMotorPower(double batteryVoltage);
 
     void SetEncoder(double positionRaw, double velocityRaw);
 };

expansionhub/main.cpp

@@ -129,10 +129,23 @@ void ExpansionHubState::SendCommands(bool canEnable, bool deviceReset) {
         }
     }
 
-    // First the 4 motors.
+    // Compute motor powers
+    std::pair<double, int> motorPowers[NUM_MOTORS_PER_HUB];
     for (int i = 0; i < NUM_MOTORS_PER_HUB; i++) {
-        currentHub->SendMotorConstantPower(
-            i, ntStore.motors[i].ComputeMotorPower(ntStore.lastBattery));
+        motorPowers[i] =
+            ntStore.motors[i].ComputeMotorPower(ntStore.lastBattery);
+    }
+
+    // Then send the 4 motors.
+    for (int i = 0; i < NUM_MOTORS_PER_HUB; i++) {
+        if (motorPowers[i].second < 0) {
+            currentHub->SendMotorConstantPower(i, motorPowers[i].first);
+        } else if (motorPowers[i].second < 4) {
+            // TODO, determine if a follower is following a follower, and what to do
+            currentHub->SendMotorConstantPower(i, motorPowers[motorPowers[i].second].first);
+        } else {
+            currentHub->SendMotorConstantPower(i, 0.0);
+        }
     }
 
     // Then get the motor currents
@@ -175,7 +188,8 @@ void ExpansionHubState::SendCommands(bool canEnable, bool deviceReset) {
     currentHub->Flush();
 }
 
-void ExpansionHubState::OnDeviceAdded(std::unique_ptr<eh::ExpansionHubSerial> hub) {
+void ExpansionHubState::OnDeviceAdded(
+    std::unique_ptr<eh::ExpansionHubSerial> hub) {
     currentHub = std::move(hub);
 
     ntStore.Initialize(*ntInstance, busId);
@@ -275,7 +289,8 @@ static void OnDeviceAdded(wpi::uv::Loop& loop,
         return;
     }
 
-    std::unique_ptr<eh::ExpansionHubSerial> sl = std::make_unique<eh::ExpansionHubSerial>();
+    std::unique_ptr<eh::ExpansionHubSerial> sl =
+        std::make_unique<eh::ExpansionHubSerial>();
     bool isInit = sl->Initialize(loop, ret, devPath);
     printf("initialized %d\n", isInit ? 1 : 0);
 
@@ -341,8 +356,8 @@ int main() {
     };
 
     bool success = false;
-    loopRunner.ExecSync([&success, &states, &loopStorage, &ntInst,
-                         &usbMonitor, &enabledState](wpi::uv::Loop& loop) {
+    loopRunner.ExecSync([&success, &states, &loopStorage, &ntInst, &usbMonitor,
+                         &enabledState](wpi::uv::Loop& loop) {
         loopStorage.loop = &loop;
         for (size_t i = 0; i < states.size(); i++) {
             success = states[i].StartUvLoop(i, ntInst, loop);