expand / unwrap for linear and rotary axis OK

controller/client/clStepClient.js

@@ -109,9 +109,9 @@ posEstPlot.setHoldCount(500)
 //tempPlot.setYDomain(0, 100)
 posEstPlot.redraw()
 
-let angleDotPlot = new AutoPlot(130, 670, 500, 210, 'angle dot')
-angleDotPlot.setHoldCount(500)
-angleDotPlot.redraw()
+let posDotPlot = new AutoPlot(130, 670, 500, 210, 'position dot')
+posDotPlot.setHoldCount(500)
+posDotPlot.redraw()
 
 let specLp = false
 let specPlotCount = 0
@@ -129,7 +129,7 @@ loopQueryBtn.onClick(() => {
           effortPlot.pushPt([specPlotCount, spec.effort]); effortPlot.redraw()
           anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw()
           posEstPlot.pushPt([specPlotCount, spec.posEstimate]); posEstPlot.redraw()
-          angleDotPlot.pushPt([specPlotCount, spec.angleDot]); angleDotPlot.redraw()
+          posDotPlot.pushPt([specPlotCount, spec.posDot]); posDotPlot.redraw()
           setTimeout(loop, 10)
         }).catch((err) => {
           console.error(err)
@@ -198,7 +198,7 @@ pidSetBtn.onClick(() => {
 
 let angleRandoBtn = new Button(10, 400, 94, 14, 'targ')
 angleRandoBtn.onClick(() => {
-  let targ = Math.random() * 360 
+  let targ = Math.random() * 200 - 100
   vm.setTarget(targ).then(() => {
     angleRandoBtn.good(`${targ.toFixed(3)}`, 250)
   }).catch((err) => {
@@ -217,6 +217,25 @@ angleWrapBtn.onClick(() => {
   })
 })
 
+// units per revolution
+
+let uprVal = new TextInput(10, 460, 100, 20, '40')
+
+let uprBtn = new Button(10, 490, 94, 14, 'upr')
+uprBtn.onClick(() => {
+  let upr = parseFloat(uprVal.value)
+  if(Number.isNaN(upr)){
+    uprBtn.bad("bad parse", 500)
+    return
+  }
+  vm.setUPR(upr).then(() => {
+    uprBtn.good("ok", 500)
+  }).catch((err) => {
+    console.error(err)
+    uprBtn.bad("err", 500)
+  })
+})
+
 // -------------------------------------------------------- STARTUP LOCAL
 
 let wscVPort = osap.vPort()

controller/client/clStepVM.js

@@ -109,7 +109,7 @@ export default function ClStepVM(osap, route) {
           encoder: TS.read('uint16', data, 0, true),
           angle: TS.read('float32', data, 2, true),
           posEstimate: TS.read('float32', data, 6, true),
-          angleDot: TS.read('float32', data, 10, true),
+          posDot: TS.read('float32', data, 10, true),
           effort: TS.read('float32', data, 14, true)
         }
         resolve(result)
@@ -149,6 +149,20 @@ export default function ClStepVM(osap, route) {
     })
   }
 
+  // ------------------------------------------------------ 8: SET UPR
+
+  let uprEP = osap.endpoint()
+  uprEP.addRoute(route, TS.endpoint(0, 8), 512)
+  this.setUPR = (upr) => {
+    return new Promise((resolve, reject) => {
+      let datagram = new Uint8Array(4)
+      TS.write('float32', upr, datagram, 0, true)
+      uprEP.write(datagram).then(() => {
+        resolve()
+      }).catch((err) => { resolve(err) })
+    })
+  }
+
   /*
   // ------------------------------------------------------ POS
   // ok: we make an 'endpoint' that will transmit moves,

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

@@ -127,6 +127,18 @@ float Step_CL::get_torque(void){
     return _tc;
 }
 
+// units per revolution
+float _upr = 1.0F;
+
+void Step_CL::set_upr(float upr){
+    _upr = upr;
+}
+
+float Step_CL::get_upr(void){
+    return _upr;
+}
+
+
 // mode set
 #define MODE_NO_OP 0 
 #define MODE_RUN_PID 1
@@ -137,8 +149,8 @@ void Step_CL::set_run_mode(uint8_t mode){
 }
 
 void Step_CL::set_pos_target(float tp){
-    clamp(&tp, 0.0F, 360.0F);
-    _tp = tp;
+    _tp = tp * (360.0F / _upr); // internal target is degrees from zero
+    // so here we translate, using the units per revolution (one full 360 degs...) 
 }
 
 float Step_CL::get_pos_target(void){
@@ -191,8 +203,8 @@ volatile int32_t revolutions = 0;      // count of revolutions,
 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 p_dot = 0.0f;
+volatile float p_dot_last = 0.0f;
 volatile float integral = 0.0f;
 
 volatile float _pa;         // phase angle 
@@ -210,7 +222,7 @@ float Step_CL::get_last_pos_est(void){
 }
 
 float Step_CL::get_last_pos_dot(void){
-    return (float)revolutions;
+    return p_dot;
 }
 
 float Step_CL::get_output_effort(void){
@@ -237,28 +249,23 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     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));
+    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); 
-    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;
+    p_dot_last = p_dot;
+    p_dot = (p_est - p_est_last) * (1000000 / US_PER_LOOP); 
+    p_dot = (p_dot * p_dot_alpha) + (p_dot_last * (1 - p_dot_alpha));
     // calc integral
-    integral += iTerm * (a_est - _ta);  // properly there would be this term: * (US_PER_LOOP / 1000000);
+    integral += iTerm * (p_est - _tp);  // properly there would be this term: * (US_PER_LOOP / 1000000);
     if(integral > iLimit){              // to make the 'integral' meaningful w/r/t time, however, this causes 
         integral = iLimit;              // what I expect are numerical instabilities or something 
     } else if (integral < (-iLimit)){   // as we end up doing like 10000 * 0.0001 * err ... idk 
         integral = -iLimit;
     }
-    */
-
+    
     // calculate output, 
-    float output = pTerm * (p_est - _tp); //+ dTerm * a_dot + integral;
+    float output = pTerm * (p_est - _tp) + dTerm * p_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

@@ -39,6 +39,9 @@ class Step_CL {
         // pid set 
         void set_pid_values(float p, float i, float ilim, float d);
         void set_alpha_values(float a, float adot);
+        // units per revolution 
+        void set_upr(float upr);
+        float get_upr(void);
         // read stat 
         uint16_t get_last_enc_reading(void);
         float get_last_pos_reading(void);

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

@@ -105,7 +105,7 @@ 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 posn_dot = step_cl->get_last_pos_dot();
   float effort = step_cl->get_output_effort();
   // ship,
   uint8_t data[18];
@@ -113,7 +113,7 @@ boolean onSpecQuery(void){
   ts_writeUint16(encoder_reading, data, &wptr);
   ts_writeFloat32(angle_reading, data, &wptr);
   ts_writeFloat32(posn, data, &wptr);
-  ts_writeFloat32(angle_dot, data, &wptr);
+  ts_writeFloat32(posn_dot, data, &wptr);
   ts_writeFloat32(effort, data, &wptr);
   specQueryEP->write(data, 18);
   return true;
@@ -145,6 +145,17 @@ boolean onAlphaPut(uint8_t* data, uint16_t len){
 
 Endpoint* alphaEP = osap->endpoint(onAlphaPut);
 
+// -------------------------------------------------------- 8: Units Per Rev
+
+boolean onUPRPut(uint8_t* data, uint16_t len){
+  uint16_t ptr = 0;
+  chunk_float32 uprc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
+  step_cl->set_upr(uprc.f);
+  return true;
+}
+
+Endpoint* uprEP = osap->endpoint(onUPRPut);
+
 // -------------------------------------------------------- SETUP / RUNTIME 
 
 void setup() {

log/cl-step-control-log.md

@@ -1395,7 +1395,14 @@ Now I can do the derivative, error, and integral all in terms of actual error on
 
 I think I'll get that back up and running... and target a wider range of positions, should be all tuned in already, and then I'm kind of finished with this step... next would be to bring it into a real machine context, benchtop test a new VM with an accel controller feeding it positions. 
 
-- re-integrate I and D terms
-- tune back in
-- set 'units per rev'
-- test, OK 
\ No newline at end of file
+So, for 'units per revolution' (1) I cannot just throw in a -ve value here to reverse it, but I would like to be able to and (2) the tuning variables depend on this, since the loop now runs in the 'space' of the unrolled position, rather than in the 'space' of the wrapped / unwrapped angles. 
+
+I think maybe then the move is to leave the controller in degrees-and-revolutions, kind of makes sense down there, and feed it target 'revs & degrees' positions based on this. Yep, this is the way. 
+
+OK, all seems sound now. So tomorrow would be about setting this up in a 'real system' ... or maybe it's prepping that system: I have parts ready to make another clank, a larger one... though I should check against the reality of my available desk space. I might as well jump in on the X axis there...
+
+- re-order the API / the internal code: lots of spaghetti in here at the moment
+- match vm api, 
+- build clank-stretch 
+- put it on the bus, check if all commands can still op. through the bus: this is higher bandwidth, check for drops etc, maybe you have to fix those now
+- configure to grab positions from the bus, you'll be mixing closed- and open-loop motors not irregularely. 
\ No newline at end of file