diff --git a/controller/client/clStepClient.js b/controller/client/clStepClient.js
index 711042b30e8410d6ff3ec23223e762fb52627dfa..8d0cea88e2ac7a7088a794c7ae40a6e1a8f6afec 100644
--- a/controller/client/clStepClient.js
+++ b/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
diff --git a/controller/client/clStepVM.js b/controller/client/clStepVM.js
index 7f2a9c6dd90d94591a2f74a8986585c3f87d5290..fa0e5487e0b90274fbef671b92f003f2a7bb9e7e 100644
--- a/controller/client/clStepVM.js
+++ b/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) })
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index c83fb97825a32c3428398a6f2d2a880d8bba13f1..72208dc68f5d6e1435664d27581607a7cb26f02d 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/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
}
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index 73a17108dc746c06d8f003740aa56121971cc6e8..177429f0fd3f1a3e8eec93030532e083c842eac8 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/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;
diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp
index 857f62a844d53ef61c101e87150a6fa433692646..b4343beaba923d2acf0ed396f89af094c877ec73 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/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
diff --git a/log/2021-02-15_angle-dot.png b/log/2021-02-15_angle-dot.png
new file mode 100644
index 0000000000000000000000000000000000000000..318604be0d15a6adf7610d4687e0948f55fccf47
Binary files /dev/null and b/log/2021-02-15_angle-dot.png differ
diff --git a/log/2021-02-15_cl-step-pid-start.mp4 b/log/2021-02-15_cl-step-pid-start.mp4
new file mode 100644
index 0000000000000000000000000000000000000000..a7ed2a2fbb8b01d01516664e028f8437e1b70bf7
Binary files /dev/null and b/log/2021-02-15_cl-step-pid-start.mp4 differ
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index fefc9aaf37acf9734ce4d773f7bf1820ec06e8b9..857c4c9665635a0d0d409c72bd2f3b4ef9ee8e5b 100644
--- a/log/cl-step-control-log.md
+++ b/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.
@@ -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.
+
+
+
+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*
+
+
+
+So, next would do:
+- PID remote-set
+- target-pos remote-set
+- tuning attempts
+- auto-tune ?
\ No newline at end of file