diff --git a/controller/client/clStepClient.js b/controller/client/clStepClient.js
index 85aad4ebc57141852b8c1f0f499ba00b341322f4..6517171a9962233a48ae85a22474163b175478a2 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 a9334d9c9076c4a3245ef7fde8400c24d341be1e..11680749d033b8fc25f64dfeaee066b8b06d6fac 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 8dca1319bccd18705ec1da52e3a13ad879893e7e..29a13b3fd4afd9bbfe18d827985290b536374069 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 e23546f151d5b1f8f77d6702769dd0064686ccb3..9b6eadbf13d7088fdf08ebf5b1308dfa415b2401 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 27afad4b3e423641330bad7ab64ea45deefaab50..ae33368882e14255a55dc08c0a4faf679aedcc70 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 6f6cc88cd9205203d4d1f44837c3e25ad9173997..8de71e11a38cc220fb38330b9e7dda4fa68b604a 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