Merge pull request #142 from CU-Robotics/feature-agnostic-feeder

Feature agnostic feeder

Author: Rishi-H

src/controls/controller.cpp

@@ -321,5 +321,5 @@ void NewFeederController::step(float reference[STATE_LEN][3], float estimate[STA
     // float outputv = pidv.filter(dt, true, false);
     // float output = outputp + outputv;
 
-    outputs[0] = -outputp;
+    outputs[0] = outputp * gear_ratios[0]; // negative because the feeder motor is reversed
 }

src/controls/estimator.cpp

@@ -546,6 +546,8 @@ void LocalEstimator::step_states(float output[CAN_MAX_MOTORS][MICRO_STATE_LEN],
 
 NewFeederEstimator::NewFeederEstimator(CANManager* _can, SensorManager* _sensor_manager, Config config_data) {
     feeder_offset = config_data.sensor_info[10][2];
+    feeder_direction = config_data.sensor_info[10][3];
+    feeder_ratio = config_data.sensor_info[10][4];
     micro_estimator = false;
     can = _can;
     sensor_manager = _sensor_manager;
@@ -560,18 +562,22 @@ void NewFeederEstimator::step_states(float output[CAN_MAX_MOTORS][MICRO_STATE_LE
     if (count == 0) {
         Serial.printf("prev_feeder_angle %f\n",prev_feeder_angle);
         dt = 0; // first dt loop generates huge time so check for that
-        diff = fmod((feeder_angle - feeder_offset), (float)(M_PI / 2.0)) ;
+        diff = fmod((feeder_angle - feeder_offset), (float)(M_PI / feeder_ratio)) ;
         count++;
     } else {
         diff = feeder_angle - prev_feeder_angle;
     }
+  
     prev_feeder_angle = feeder_angle;
     if (diff > PI) diff -= 2 * PI;
     else if (diff < -PI) diff += 2 * PI;
-    float feeder_velocity = (dt > 0) ? (diff/(M_PI/2.0))/dt : 0;
-    ball_count += diff/(M_PI/2.0);
-    output[0][0] = ball_count;
-    output[0][1] = feeder_velocity;
+  
+    float feeder_velocity = (dt > 0) ? (diff/(M_PI/feeder_ratio))/dt : 0;
+  
+    ball_count += diff/(M_PI/feeder_ratio);
+    output[0][0] = ball_count * feeder_direction; // ball count
+    output[0][1] = feeder_velocity * feeder_direction; // ball velocity
     output[0][2] = feeder_angle; // this is not the acceleration just the encoder value for debugging
+
 }
 

src/controls/estimator.hpp

@@ -561,6 +561,11 @@ struct NewFeederEstimator : public Estimator {
         int count = 0;
         /// @brief feeder encoder offset for improving feeder delay
         float feeder_offset = 0;
+        /// @brief feeder direction multiplier
+        /// @note 1 for normal direction, -1 for reverse direction
+        float feeder_direction = 1;
+        /// @brief gear ratio of the feeder
+        float feeder_ratio = 1;
     public:
         /// @brief Make new local estimator and set can data pointer and num states
         /// @param can can pointer from EstimatorManager

src/main.cpp

@@ -433,6 +433,7 @@ int main() {
             can.issue_safety_mode();
             governor.set_reference_at_index(temp_state[6][0], 6, 0);
             feed = temp_state[6][0]; // reset feed to the current state
+            last_feed = feed; // reset last feed to the current state
             // Serial.printf("Can zero\n");
         }