basic wrap works

controller/client/clStepClient.js

@@ -104,10 +104,10 @@ anglePlot.setHoldCount(500)
 //tempPlot.setYDomain(0, 100)
 anglePlot.redraw()
 
-let angleEstPlot = new AutoPlot(130, 450, 500, 210, 'angle estimate')
-angleEstPlot.setHoldCount(500)
+let posEstPlot = new AutoPlot(130, 450, 500, 210, 'pos estimate')
+posEstPlot.setHoldCount(500)
 //tempPlot.setYDomain(0, 100)
-angleEstPlot.redraw()
+posEstPlot.redraw()
 
 let angleDotPlot = new AutoPlot(130, 670, 500, 210, 'angle dot')
 angleDotPlot.setHoldCount(500)
@@ -128,7 +128,7 @@ loopQueryBtn.onClick(() => {
           specPlotCount++
           effortPlot.pushPt([specPlotCount, spec.effort]); effortPlot.redraw()
           anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw()
-          angleEstPlot.pushPt([specPlotCount, spec.angleEstimate]); angleEstPlot.redraw()
+          posEstPlot.pushPt([specPlotCount, spec.posEstimate]); posEstPlot.redraw()
           angleDotPlot.pushPt([specPlotCount, spec.angleDot]); angleDotPlot.redraw()
           setTimeout(loop, 10)
         }).catch((err) => {

controller/client/clStepVM.js

@@ -108,7 +108,7 @@ export default function ClStepVM(osap, route) {
         let result = {
           encoder: TS.read('uint16', data, 0, true),
           angle: TS.read('float32', data, 2, true),
-          angleEstimate: TS.read('float32', data, 6, true),
+          posEstimate: TS.read('float32', data, 6, true),
           angleDot: TS.read('float32', data, 10, true),
           effort: TS.read('float32', data, 14, true)
         }

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

@@ -114,7 +114,7 @@ void Step_CL::print_table(void){
 // targets, 
 
 volatile float _tc = 0.0F;  // torque output 
-volatile float _ta = 180.0F;  // angular target 
+volatile float _tp = 180.0F;  // position target 
 
 // set twerks 
 // tc: -1 : 1
@@ -136,13 +136,13 @@ 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;
+void Step_CL::set_pos_target(float tp){
+    clamp(&tp, 0.0F, 360.0F);
+    _tp = tp;
 }
 
-float Step_CL::get_angle_target(void){
-    return _ta;
+float Step_CL::get_pos_target(void){
+    return _tp;
 }
 
 // PIDs
@@ -160,12 +160,12 @@ void Step_CL::set_pid_values(float p, float i, float ilim, float d){
     dTerm = d;
 }
 
-volatile float a_alpha = 0.2F;       // trust in measurement 
-volatile float a_dot_alpha = 0.05F;
+volatile float p_alpha = 0.2F;       // trust in measurement 
+volatile float p_dot_alpha = 0.05F;
 
 void Step_CL::set_alpha_values(float a, float adot){
-    a_alpha = a;
-    a_dot_alpha = adot;
+    p_alpha = a;
+    p_dot_alpha = adot;
 }
 
 // the control loop 
@@ -182,11 +182,15 @@ void Step_CL::run_torque_loop(void){
 #define TICKS_PER_SEC 50000.0F
 #define SECS_PER_TICK 1.0F / TICKS_PER_SEC
 
-volatile uint16_t enc_reading = 0;
-volatile float a_reading = 0.0F;
-volatile float last_measurement = 0.0F;
-volatile float a_est = 0.0F;
-volatile float a_last_est = 0.0F;
+// encoder / revoutions-wise,
+volatile uint16_t enc_reading = 0;      // 14 bit encoder reading
+volatile float a_reading = 0.0F;        // fp angular measurement, given lut 
+volatile float a_reading_last = 0.0F;   // last-loop's angular measurement 
+volatile int32_t revolutions = 0;      // count of revolutions, 
+// linear or rotary position wise,
+volatile float p_est = 0.0F;
+volatile float p_est_last = 0.0F;
+
 volatile float a_dot = 0.0f;
 volatile float a_dot_last = 0.0f;
 volatile float integral = 0.0f;
@@ -202,11 +206,11 @@ float Step_CL::get_last_pos_reading(void){
 }
 
 float Step_CL::get_last_pos_est(void){
-    return a_est;
+    return p_est;
 }
 
 float Step_CL::get_last_pos_dot(void){
-    return a_dot;
+    return (float)revolutions;
 }
 
 float Step_CL::get_output_effort(void){
@@ -218,10 +222,25 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     // stash this,
     enc_reading = result;
     // stash old measurement, pull new from LUT 
-    last_measurement = a_reading;
+    a_reading_last = a_reading;
     a_reading = lut[result * 2]; 
-    // make estimate 
-    a_est = (a_reading * a_alpha) + (a_last_est * (1 - a_alpha));
+    // check for wraps around the circle,
+    // i.e. if we went from 359 -> 1 degs, we have gone around once, 
+    // if we went from 1 -> 359, we have decrimented one rev 
+    if(a_reading - a_reading_last > 180.0F){
+        revolutions --;
+    } else if (a_reading_last - a_reading > 180.0F){
+        revolutions ++;
+    }
+
+    // now we have a real position measurement:
+    p_est_last = p_est;
+    p_est = (float)revolutions * 360.0F + a_reading;
+    // and so can make an estimate of this, 
+    //p_est = (p_est * p_alpha) + (p_est_last * (1 - p_alpha));
+
+    // omit this for now, 
+    /*
     // derivative, 
     // 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); 
@@ -236,9 +255,10 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     } else if (integral < (-iLimit)){   // as we end up doing like 10000 * 0.0001 * err ... idk 
         integral = -iLimit;
     }
+    */
 
     // calculate output, 
-    float output = pTerm * (a_est - _ta) + dTerm * a_dot + integral;
+    float output = pTerm * (p_est - _tp); //+ dTerm * a_dot + integral;
     effort = output;
     clamp(&output, -1.0F, 1.0F);
     // _tc (nominally torque output / effort) is a local variable to this file scope, 

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

@@ -33,8 +33,8 @@ class Step_CL {
         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);
+        void set_pos_target(float ta);
+        float get_pos_target(void);
         float get_output_effort(void);
         // pid set 
         void set_pid_values(float p, float i, float ilim, float d);

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

@@ -66,8 +66,8 @@ boolean onPositionTargetPut(uint8_t* data, uint16_t len);
 Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut);
 
 boolean onPositionTargetPut(uint8_t* data, uint16_t len){
-  chunk_float32 tac = { .bytes = { data[0], data[1], data[2], data[3] }};
-  step_cl->set_angle_target(tac.f);
+  chunk_float32 tpc = { .bytes = { data[0], data[1], data[2], data[3] }};
+  step_cl->set_pos_target(tpc.f);
   return true;
 }
 

log/cl-step-control-log.md

@@ -1321,6 +1321,8 @@ D       -0.0003
 
 A       0.2
 A*      0.05
+
+at 10kHz 
 ```
 
 OK, I am satisfied with these for the time being, time now is to apply it to a real system. I have also a few things I want to know:
@@ -1352,4 +1354,17 @@ So, then, next is having target set-points that are not directly related to the
 So! There must be an established way to do this... 
 
 - wrap around 360, test with 'targ 360' key 
-- supervisor for offsets / etc ? 
+- supervisor for offsets / etc ? 
\ No newline at end of file
+
+Wondering if this is going to get tricky.
+
+- derivative should also 'wrap' - otherwise there will be a spike between 0.0 and 360.0.
+
+Competing approaches would be 
+
+- wrap around 360 in the subsystem, supervisor points to angles around 360 
+- or, count angle and *revolutions (integer, 32bit)* and track current-pos of these... translate between floating point *target* pos and *revs + angle* with units per step scalar 
+
+I think the second manner is actually best, so I'm giving that a shot. First step I take is to translate from rotary measurements -> linear / unrolled-rotary position, wrapping around & incrementing a 'revolutions' count, then I calculate 'real position' with revs * angular_pos. 
+
+Just taking a minute to figure out the particulars on this one... `a r i t h m e t i c` 
\ No newline at end of file