From 7ff7ea21f0f4741480c7371b2464b185fcc94794 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Wed, 17 Feb 2021 17:33:05 -0500
Subject: [PATCH] expand / unwrap for linear and rotary axis OK
---
controller/client/clStepClient.js | 29 ++++++++++---
controller/client/clStepVM.js | 16 +++++++-
.../src/drivers/step_cl.cpp | 41 +++++++++++--------
.../cl-step-controller/src/drivers/step_cl.h | 3 ++
firmware/cl-step-controller/src/main.cpp | 15 ++++++-
log/cl-step-control-log.md | 15 +++++--
6 files changed, 90 insertions(+), 29 deletions(-)
diff --git a/controller/client/clStepClient.js b/controller/client/clStepClient.js
index 85aad4e..6517171 100644
--- a/controller/client/clStepClient.js
+++ b/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()
diff --git a/controller/client/clStepVM.js b/controller/client/clStepVM.js
index a9334d9..1168074 100644
--- a/controller/client/clStepVM.js
+++ b/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,
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 8dca131..29a13b3 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/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,
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index e23546f..9b6eadb 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/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);
diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp
index 27afad4..ae33368 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/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() {
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index 6f6cc88..8de71e1 100644
--- a/log/cl-step-control-log.md
+++ b/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
--
GitLab