up to PID runtime

controller/client/clStepClient.js

@@ -94,10 +94,10 @@ positionQueryBtn.onClick(() => {
 
 // -------------------------------------------------------- LOOP SPEC
 
-let encPlot = new AutoPlot(130, 10, 500, 210, 'encoder')
-encPlot.setHoldCount(500)
+let effortPlot = new AutoPlot(130, 10, 500, 210, 'effort')
+effortPlot.setHoldCount(500)
 //tempPlot.setYDomain(0, 100)
-encPlot.redraw()
+effortPlot.redraw()
 
 let anglePlot = new AutoPlot(130, 230, 500, 210, 'angle reading')
 anglePlot.setHoldCount(500)
@@ -126,9 +126,10 @@ loopQueryBtn.onClick(() => {
       } else {
         vm.getSpec().then((spec) => {
           specPlotCount++
-          encPlot.pushPt([specPlotCount, spec.encoder]); encPlot.redraw()
+          effortPlot.pushPt([specPlotCount, spec.effort]); effortPlot.redraw()
           anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw()
           angleEstPlot.pushPt([specPlotCount, spec.angleEstimate]); angleEstPlot.redraw()
+          angleDotPlot.pushPt([specPlotCount, spec.angleDot]); angleDotPlot.redraw()
           setTimeout(loop, 10)
         }).catch((err) => {
           console.error(err)
@@ -142,6 +143,28 @@ loopQueryBtn.onClick(() => {
   }
 })
 
+// -------------------------------------------------------- RUN / MODE SWICH
+
+let runBtn = new Button(10, 130, 94, 14, 'run')
+runBtn.onClick(() => {
+  vm.setMode("pid").then(() =>{
+    runBtn.good("ok", 500)
+  }).catch((err) => {
+    console.error(err)
+    runBtn.bad("err", 500)
+  })
+})
+
+let stopBtn = new Button(10, 160, 94, 14, 'stop')
+stopBtn.onClick(() => {
+  vm.setMode("noop").then(() =>{
+    stopBtn.good("ok", 500)
+  }).catch((err) => {
+    console.error(err)
+    stopBtn.bad("err", 500)
+  })
+})
+
 // -------------------------------------------------------- MOTION FEED
 
 // -------------------------------------------------------- PID 

controller/client/clStepVM.js

@@ -49,9 +49,35 @@ export default function ClStepVM(osap, route) {
     })
   }
 
-  // ------------------------------------------------------ 3: POSITION QUERY
+  // ------------------------------------------------------ 2: SET MODE SWITCH
 
-  let positionQuery = osap.query(route, TS.endpoint(0, 3), 512)
+  let modeEP = osap.endpoint()
+  modeEP.addRoute(route, TS.endpoint(0, 2), 512)
+  this.setMode = (mode) => {
+    return new Promise((resolve, reject) => {
+      let val = 0
+      switch (mode) {
+        case "noop":
+          val = 0
+          break;
+        case "pid":
+          val = 1
+          break;
+        default:
+          reject("bad mode key");
+          return
+      }
+      modeEP.write(Uint8Array.from([val])).then(() => {
+        resolve()
+      }).catch((err) => { reject(err) })
+    })
+  }
+
+  // ------------------------------------------------------ 3: SET POSITION TARGET  
+
+  // ------------------------------------------------------ 4: POSITION QUERY
+
+  let positionQuery = osap.query(route, TS.endpoint(0, 4), 512)
   this.getPosition = () => {
     return new Promise((resolve, reject) => {
       positionQuery.pull().then((data) => {
@@ -61,16 +87,18 @@ export default function ClStepVM(osap, route) {
     })
   }
 
-  // ------------------------------------------------------ 4: LOOP SPEC
+  // ------------------------------------------------------ 5: LOOP SPEC
 
-  let specQuery = osap.query(route, TS.endpoint(0, 4), 512)
+  let specQuery = osap.query(route, TS.endpoint(0, 5), 512)
   this.getSpec = () => {
     return new Promise((resolve, reject) => {
       specQuery.pull().then((data) => {
         let result = {
           encoder: TS.read('uint16', data, 0, true),
           angle: TS.read('float32', data, 2, true),
-          angleEstimate: TS.read('float32', data, 6, true)
+          angleEstimate: TS.read('float32', data, 6, true),
+          angleDot: TS.read('float32', data, 10, true),
+          effort: TS.read('float32', data, 14, true)
         }
         resolve(result)
       }).catch((err) => { reject(err) })

firmware/cl-step-controller/src/drivers/step_cl.cpp

@@ -15,6 +15,7 @@ is; no warranty is provided, and users accept all liability.
 #include "step_cl.h"
 #include "../utils/FlashStorage.h"
 #include "../utils/clamp.h"
+#include "../osape/utils/clocks_d51.h"
 
 Step_CL* Step_CL::instance = 0;
 
@@ -36,11 +37,21 @@ Step_CL::Step_CL(void){}
 
 #define ENCODER_COUNTS 16384
 
+#define US_PER_LOOP 100
+
 void Step_CL::init(void){
     step_a4950->init(false, 0.4);
     enc_as5047->init();
-    _tc = 0; // torque command -> 0; 
     is_calibrating = false;
+    // start the timer, 
+    d51_clock_boss->start_ticker_a(US_PER_LOOP);
+}
+
+// on ticker_a 
+void TC0_Handler(void){
+  TC0->COUNT32.INTFLAG.bit.MC0 = 1;
+  TC0->COUNT32.INTFLAG.bit.MC1 = 1;
+  step_cl->run_torque_loop();
 }
 
 // LUT / flash work 
@@ -100,6 +111,11 @@ void Step_CL::print_table(void){
     }
 }
 
+// targets, 
+
+volatile float _tc = 0.0F;  // torque output 
+volatile float _ta = 180.0F;  // angular target 
+
 // set twerks 
 // tc: -1 : 1
 void Step_CL::set_torque(float tc){
@@ -111,6 +127,30 @@ float Step_CL::get_torque(void){
     return _tc;
 }
 
+// mode set
+#define MODE_NO_OP 0 
+#define MODE_RUN_PID 1
+volatile uint8_t _mode = MODE_NO_OP;
+
+void Step_CL::set_run_mode(uint8_t mode){
+    _mode = mode;
+}
+
+void Step_CL::set_angle_target(float ta){
+    clamp(&ta, 0.0F, 360.0F);
+    _ta = ta;
+}
+
+float Step_CL::get_angle_target(void){
+    return _ta;
+}
+
+// PIDs
+
+volatile float pTerm = -0.2F;
+volatile float dTerm = -0.0F;
+volatile float effort = 0.0F;
+
 // the control loop 
 void Step_CL::run_torque_loop(void){
     if(is_calibrating) return;
@@ -131,6 +171,9 @@ volatile float last_measurement = 0.0F;
 volatile float a_est = 0.0F;
 volatile float a_last_est = 0.0F;
 volatile float a_alpha = 0.01F;       // trust in measurement 
+volatile float a_dot = 0.0f;
+volatile float a_dot_last = 0.0f;
+volatile float a_dot_alpha = 0.1F;
 
 volatile float _pa;         // phase angle 
 
@@ -146,6 +189,14 @@ float Step_CL::get_last_pos_est(void){
     return a_est;
 }
 
+float Step_CL::get_last_pos_dot(void){
+    return a_dot;
+}
+
+float Step_CL::get_output_effort(void){
+    return effort;
+}
+
 void ENC_AS5047::on_read_complete(uint16_t result){
     if(step_cl->is_calibrating) return;
     // stash this,
@@ -155,20 +206,36 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     a_reading = lut[result * 2]; 
     // make estimate 
     a_est = (a_reading * a_alpha) + (a_last_est * (1 - a_alpha));
-    a_last_est = a_est;
     // derivative, 
-    
-    // or like 
-    // I am completely confused by the noise that this generates ? seems like it's taking a difference?
-    // should just be averaging? 
-    // something about getting the variables out, under interrupt? 
-    //a_est = a_reading; // 0.5 * a_measurement + 0.5 * last_measurement;
-    //output = alpha * (input - output) + output 
+    // compiler should turn second term into const, and * reciprocal of timebase, rather than /, for no div 
+    a_dot = (a_est - a_last_est) * (1000000 / US_PER_LOOP); 
+    a_dot = (a_dot * a_dot_alpha) + (a_dot_last * (1 - a_dot_alpha));
+    // set history 
+    a_last_est = a_est;
+    a_dot_last = a_dot;
+
+    // calculate output, 
+    float output = pTerm * (a_est - _ta) + dTerm * a_dot;
+    effort = output;
+    clamp(&output, -1.0F, 1.0F);
+    // _tc (nominally torque output / effort) is a local variable to this file scope, 
+    // accesible externally with set / get_torque 
+    // ibid _ta, angle target 
+    switch(_mode){
+        case MODE_RUN_PID:
+            _tc = output;
+            break;
+        case MODE_NO_OP:
+        default:
+            _tc = 0.0F;
+            break;
+    }
+    //_tc = output;
 
     // this commutation phase is independent from the above, more or less 
     _pa = lut[result * 2 + 1];      // the phase angle (0 - 1 in a sweep of 4 steps)
     // this is the phase angle we want to apply, 90 degs off & wrap't to 1 
-    if(step_cl->get_torque() < 0){
+    if(_tc < 0){
         _pa -= 0.25; // 90* phase swop 
         if(_pa < 0){
             _pa += 1.0F;
@@ -181,7 +248,7 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     }
     // now we ask our voltage modulation machine to put this on the coils 
     // with the *amount* commanded by our _tc torque ask 
-    step_a4950->point(_pa, abs(step_cl->get_torque()));
+    step_a4950->point(_pa, abs(_tc));
     // debug loop completion 
 }
 

firmware/cl-step-controller/src/drivers/step_cl.h

@@ -24,7 +24,6 @@ class Step_CL {
     private:
         static Step_CL* instance;
         float calib_readings[201];
-        volatile float _tc;
 
     public:
         Step_CL();
@@ -33,10 +32,16 @@ class Step_CL {
         void print_table(void);
         void set_torque(float tc);
         float get_torque(void);
+        void set_run_mode(uint8_t mode); 
+        void set_angle_target(float ta);
+        float get_angle_target(void);
+        float get_output_effort(void);
         // read stat 
         uint16_t get_last_enc_reading(void);
         float get_last_pos_reading(void);
         float get_last_pos_est(void);
+        float get_last_pos_dot(void);
+        // do work 
         void run_torque_loop(void);
         boolean calibrate(void);
         boolean is_calibrating;

firmware/cl-step-controller/src/main.cpp

@@ -51,7 +51,16 @@ boolean onRunCalibPut(uint8_t* data, uint16_t len){
   return true;
 }
 
-// -------------------------------------------------------- 2: POSITION TARGET 
+// -------------------------------------------------------- 2: MODE SWITCH 
+
+boolean onRunModePut(uint8_t* data, uint16_t len){
+  step_cl->set_run_mode(data[0]);
+  return true;
+}
+
+Endpoint* runModeEP = osap->endpoint(onRunModePut);
+
+// -------------------------------------------------------- 3: POSITION TARGET 
 
 boolean onPositionTargetPut(uint8_t* data, uint16_t len);
 Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut);
@@ -60,7 +69,7 @@ boolean onPositionTargetPut(uint8_t* data, uint16_t len){
   return true;
 }
 
-// -------------------------------------------------------- 3: CURRENT POSITION
+// -------------------------------------------------------- 4: CURRENT POSITION
 
 boolean onPositionPut(uint8_t* data, uint16_t len);
 boolean onPositionQuery(void);
@@ -79,7 +88,7 @@ boolean onPositionQuery(void){
   return true;
 }
 
-// -------------------------------------------------------- 4: FULL SPEC
+// -------------------------------------------------------- 5: FULL SPEC
 
 boolean onSpecPut(uint8_t* data, uint16_t len);
 boolean onSpecQuery(void);
@@ -94,13 +103,17 @@ boolean onSpecQuery(void){
   uint16_t encoder_reading = step_cl->get_last_enc_reading();
   float angle_reading = step_cl->get_last_pos_reading();
   float posn = step_cl->get_last_pos_est();
+  float angle_dot = step_cl->get_last_pos_dot();
+  float effort = step_cl->get_output_effort();
   // ship,
-  uint8_t data[10];
+  uint8_t data[18];
   uint16_t wptr = 0;
   ts_writeUint16(encoder_reading, data, &wptr);
   ts_writeFloat32(angle_reading, data, &wptr);
   ts_writeFloat32(posn, data, &wptr);
-  specQueryEP->write(data, 10);
+  ts_writeFloat32(angle_dot, data, &wptr);
+  ts_writeFloat32(effort, data, &wptr);
+  specQueryEP->write(data, 18);
   return true;
 }
 
@@ -116,15 +129,6 @@ void setup() {
   osap->addVPort(vPortSerial);
   // cl controller 
   step_cl->init();
-  // start ctrl timer for loop (arg to start is us period)
-  d51_clock_boss->start_ticker_a(100);
-}
-
-// on ticker_a 
-void TC0_Handler(void){
-  TC0->COUNT32.INTFLAG.bit.MC0 = 1;
-  TC0->COUNT32.INTFLAG.bit.MC1 = 1;
-  step_cl->run_torque_loop();
 }
 
 // async loop 

log/cl-step-control-log.md

@@ -1255,7 +1255,7 @@ I should check against position readings. Sorted those, seems like it *should* b
 
 OK, I'm pulling back a full spec / object - and yeah, big / compound typing is really really critical for these things - and I just need now to bring calibration back online, then I can get to noise-bughunting. 
 
-So I'm pulling back data now: encoder reading, an angle measurement (from the LUT) and then a filtered value ... I absolutely pinched the filter: alpha is 0.01, but this actually works really well. Here's the encoder reading up top (where I have this terrible noise... I figure a bit is going missing at some place, idk - it's an odd pattern), the measured angle beneath, and filtered finally below. This is picking out really nice variations between 320.22 deg to 320.170 - and it feels fast enough (even with the big filter) to control with. So I'll try to start with this as a reference signal. 
+So I'm pulling back data now: encoder reading, an angle measurement (from the LUT) and then a filtered value ... I absolutely pinched the filter: alpha is `0.01`, but this actually works really well. Here's the encoder reading up top (where I have this terrible noise... I figure a bit is going missing at some place, idk - it's an odd pattern), the measured angle beneath, and filtered finally below. This is picking out really nice variations between 320.22 deg to 320.170 - and it feels fast enough (even with the big filter) to control with. So I'll try to start with this as a reference signal. 
 
 ![heavy-filt](2021-02-15_heavy-alpha-0-01.png)
 
@@ -1263,7 +1263,20 @@ I should eventually try to delete these spurious signals, but... I can approach
 
 ### Position PID
 
-- write & plot a derivative term 
-- scratch a PD controller for angular position 
-- attempt to tune, visualize output effort 
-- remote-set PID terms 
\ No newline at end of file
+This is pretty straightforward, but I find noise here too - filtering the position estimate isn't really enough, I think I need to filter the derivative estimate as well after calculating - which shouldn't really be true. Eventually, surely, I need to get rid of these periodic spikes.
+
+![d-term](2021-02-15_angle-dot.png)
+
+This should be smoooother... but I think I can probably get something running besides. 
+
+OK, it's up and ready to get kicked. I need an enable-pid-control flag and ideally some way to push the target around. 
+
+Alright, here's day on PID then... still have some interface to build for PID remote set, also would like alpha settings on a_est and a_dot... but it does do *something* 
+
+![video](2021-02-15_cl-step-pid-start.mp4) 
+
+So, next would do:
+- PID remote-set
+- target-pos remote-set 
+- tuning attempts 
+- auto-tune ? 
\ No newline at end of file