Commit 95764efa authored by Jake Read's avatar Jake Read
Browse files

up to PID runtime

parent 0bba9053
...@@ -94,10 +94,10 @@ positionQueryBtn.onClick(() => { ...@@ -94,10 +94,10 @@ positionQueryBtn.onClick(() => {
// -------------------------------------------------------- LOOP SPEC // -------------------------------------------------------- LOOP SPEC
let encPlot = new AutoPlot(130, 10, 500, 210, 'encoder') let effortPlot = new AutoPlot(130, 10, 500, 210, 'effort')
encPlot.setHoldCount(500) effortPlot.setHoldCount(500)
//tempPlot.setYDomain(0, 100) //tempPlot.setYDomain(0, 100)
encPlot.redraw() effortPlot.redraw()
let anglePlot = new AutoPlot(130, 230, 500, 210, 'angle reading') let anglePlot = new AutoPlot(130, 230, 500, 210, 'angle reading')
anglePlot.setHoldCount(500) anglePlot.setHoldCount(500)
...@@ -126,9 +126,10 @@ loopQueryBtn.onClick(() => { ...@@ -126,9 +126,10 @@ loopQueryBtn.onClick(() => {
} else { } else {
vm.getSpec().then((spec) => { vm.getSpec().then((spec) => {
specPlotCount++ specPlotCount++
encPlot.pushPt([specPlotCount, spec.encoder]); encPlot.redraw() effortPlot.pushPt([specPlotCount, spec.effort]); effortPlot.redraw()
anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw() anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw()
angleEstPlot.pushPt([specPlotCount, spec.angleEstimate]); angleEstPlot.redraw() angleEstPlot.pushPt([specPlotCount, spec.angleEstimate]); angleEstPlot.redraw()
angleDotPlot.pushPt([specPlotCount, spec.angleDot]); angleDotPlot.redraw()
setTimeout(loop, 10) setTimeout(loop, 10)
}).catch((err) => { }).catch((err) => {
console.error(err) console.error(err)
...@@ -142,6 +143,28 @@ loopQueryBtn.onClick(() => { ...@@ -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 // -------------------------------------------------------- MOTION FEED
// -------------------------------------------------------- PID // -------------------------------------------------------- PID
......
...@@ -49,9 +49,35 @@ export default function ClStepVM(osap, route) { ...@@ -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 = () => { this.getPosition = () => {
return new Promise((resolve, reject) => { return new Promise((resolve, reject) => {
positionQuery.pull().then((data) => { positionQuery.pull().then((data) => {
...@@ -61,16 +87,18 @@ export default function ClStepVM(osap, route) { ...@@ -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 = () => { this.getSpec = () => {
return new Promise((resolve, reject) => { return new Promise((resolve, reject) => {
specQuery.pull().then((data) => { specQuery.pull().then((data) => {
let result = { let result = {
encoder: TS.read('uint16', data, 0, true), encoder: TS.read('uint16', data, 0, true),
angle: TS.read('float32', data, 2, 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) resolve(result)
}).catch((err) => { reject(err) }) }).catch((err) => { reject(err) })
......
...@@ -15,6 +15,7 @@ is; no warranty is provided, and users accept all liability. ...@@ -15,6 +15,7 @@ is; no warranty is provided, and users accept all liability.
#include "step_cl.h" #include "step_cl.h"
#include "../utils/FlashStorage.h" #include "../utils/FlashStorage.h"
#include "../utils/clamp.h" #include "../utils/clamp.h"
#include "../osape/utils/clocks_d51.h"
Step_CL* Step_CL::instance = 0; Step_CL* Step_CL::instance = 0;
...@@ -36,11 +37,21 @@ Step_CL::Step_CL(void){} ...@@ -36,11 +37,21 @@ Step_CL::Step_CL(void){}
#define ENCODER_COUNTS 16384 #define ENCODER_COUNTS 16384
#define US_PER_LOOP 100
void Step_CL::init(void){ void Step_CL::init(void){
step_a4950->init(false, 0.4); step_a4950->init(false, 0.4);
enc_as5047->init(); enc_as5047->init();
_tc = 0; // torque command -> 0;
is_calibrating = false; 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 // LUT / flash work
...@@ -100,6 +111,11 @@ void Step_CL::print_table(void){ ...@@ -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 // set twerks
// tc: -1 : 1 // tc: -1 : 1
void Step_CL::set_torque(float tc){ void Step_CL::set_torque(float tc){
...@@ -111,6 +127,30 @@ float Step_CL::get_torque(void){ ...@@ -111,6 +127,30 @@ float Step_CL::get_torque(void){
return _tc; 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 // the control loop
void Step_CL::run_torque_loop(void){ void Step_CL::run_torque_loop(void){
if(is_calibrating) return; if(is_calibrating) return;
...@@ -131,6 +171,9 @@ volatile float last_measurement = 0.0F; ...@@ -131,6 +171,9 @@ volatile float last_measurement = 0.0F;
volatile float a_est = 0.0F; volatile float a_est = 0.0F;
volatile float a_last_est = 0.0F; volatile float a_last_est = 0.0F;
volatile float a_alpha = 0.01F; // trust in measurement 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 volatile float _pa; // phase angle
...@@ -146,6 +189,14 @@ float Step_CL::get_last_pos_est(void){ ...@@ -146,6 +189,14 @@ float Step_CL::get_last_pos_est(void){
return a_est; 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){ void ENC_AS5047::on_read_complete(uint16_t result){
if(step_cl->is_calibrating) return; if(step_cl->is_calibrating) return;
// stash this, // stash this,
...@@ -155,20 +206,36 @@ void ENC_AS5047::on_read_complete(uint16_t result){ ...@@ -155,20 +206,36 @@ void ENC_AS5047::on_read_complete(uint16_t result){
a_reading = lut[result * 2]; a_reading = lut[result * 2];
// make estimate // make estimate
a_est = (a_reading * a_alpha) + (a_last_est * (1 - a_alpha)); a_est = (a_reading * a_alpha) + (a_last_est * (1 - a_alpha));
a_last_est = a_est;
// derivative, // derivative,
// compiler should turn second term into const, and * reciprocal of timebase, rather than /, for no div
// or like a_dot = (a_est - a_last_est) * (1000000 / US_PER_LOOP);
// I am completely confused by the noise that this generates ? seems like it's taking a difference? a_dot = (a_dot * a_dot_alpha) + (a_dot_last * (1 - a_dot_alpha));
// should just be averaging? // set history
// something about getting the variables out, under interrupt? a_last_est = a_est;
//a_est = a_reading; // 0.5 * a_measurement + 0.5 * last_measurement; a_dot_last = a_dot;
//output = alpha * (input - output) + output
// 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 // 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) _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 // 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 _pa -= 0.25; // 90* phase swop
if(_pa < 0){ if(_pa < 0){
_pa += 1.0F; _pa += 1.0F;
...@@ -181,7 +248,7 @@ void ENC_AS5047::on_read_complete(uint16_t result){ ...@@ -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 // now we ask our voltage modulation machine to put this on the coils
// with the *amount* commanded by our _tc torque ask // 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 // debug loop completion
} }
......
...@@ -24,7 +24,6 @@ class Step_CL { ...@@ -24,7 +24,6 @@ class Step_CL {
private: private:
static Step_CL* instance; static Step_CL* instance;
float calib_readings[201]; float calib_readings[201];
volatile float _tc;
public: public:
Step_CL(); Step_CL();
...@@ -33,10 +32,16 @@ class Step_CL { ...@@ -33,10 +32,16 @@ class Step_CL {
void print_table(void); void print_table(void);
void set_torque(float tc); void set_torque(float tc);
float get_torque(void); 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 // read stat
uint16_t get_last_enc_reading(void); uint16_t get_last_enc_reading(void);
float get_last_pos_reading(void); float get_last_pos_reading(void);
float get_last_pos_est(void); float get_last_pos_est(void);
float get_last_pos_dot(void);
// do work
void run_torque_loop(void); void run_torque_loop(void);
boolean calibrate(void); boolean calibrate(void);
boolean is_calibrating; boolean is_calibrating;
......
...@@ -51,7 +51,16 @@ boolean onRunCalibPut(uint8_t* data, uint16_t len){ ...@@ -51,7 +51,16 @@ boolean onRunCalibPut(uint8_t* data, uint16_t len){
return true; 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); boolean onPositionTargetPut(uint8_t* data, uint16_t len);
Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut); Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut);
...@@ -60,7 +69,7 @@ boolean onPositionTargetPut(uint8_t* data, uint16_t len){ ...@@ -60,7 +69,7 @@ boolean onPositionTargetPut(uint8_t* data, uint16_t len){
return true; return true;
} }
// -------------------------------------------------------- 3: CURRENT POSITION // -------------------------------------------------------- 4: CURRENT POSITION
boolean onPositionPut(uint8_t* data, uint16_t len); boolean onPositionPut(uint8_t* data, uint16_t len);
boolean onPositionQuery(void); boolean onPositionQuery(void);
...@@ -79,7 +88,7 @@ boolean onPositionQuery(void){ ...@@ -79,7 +88,7 @@ boolean onPositionQuery(void){
return true; return true;
} }
// -------------------------------------------------------- 4: FULL SPEC // -------------------------------------------------------- 5: FULL SPEC
boolean onSpecPut(uint8_t* data, uint16_t len); boolean onSpecPut(uint8_t* data, uint16_t len);
boolean onSpecQuery(void); boolean onSpecQuery(void);
...@@ -94,13 +103,17 @@ boolean onSpecQuery(void){ ...@@ -94,13 +103,17 @@ boolean onSpecQuery(void){
uint16_t encoder_reading = step_cl->get_last_enc_reading(); uint16_t encoder_reading = step_cl->get_last_enc_reading();
float angle_reading = step_cl->get_last_pos_reading(); float angle_reading = step_cl->get_last_pos_reading();
float posn = step_cl->get_last_pos_est(); 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, // ship,
uint8_t data[10]; uint8_t data[18];
uint16_t wptr = 0; uint16_t wptr = 0;
ts_writeUint16(encoder_reading, data, &wptr); ts_writeUint16(encoder_reading, data, &wptr);
ts_writeFloat32(angle_reading, data, &wptr); ts_writeFloat32(angle_reading, data, &wptr);
ts_writeFloat32(posn, 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; return true;
} }
...@@ -116,15 +129,6 @@ void setup() { ...@@ -116,15 +129,6 @@ void setup() {
osap->addVPort(vPortSerial); osap->addVPort(vPortSerial);
// cl controller // cl controller
step_cl->init(); 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 // async loop
......
...@@ -1255,7 +1255,7 @@ I should check against position readings. Sorted those, seems like it *should* b ...@@ -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. 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.
![heavy-filt](2021-02-15_heavy-alpha-0-01.png) ![heavy-filt](2021-02-15_heavy-alpha-0-01.png)
...@@ -1263,7 +1263,20 @@ I should eventually try to delete these spurious signals, but... I can approach ...@@ -1263,7 +1263,20 @@ I should eventually try to delete these spurious signals, but... I can approach
### Position PID ### Position PID
- write & plot a derivative term 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.
- scratch a PD controller for angular position
- attempt to tune, visualize output effort ![d-term](2021-02-15_angle-dot.png)
- remote-set PID terms
\ No newline at end of file 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*
![video](2021-02-15_cl-step-pid-start.mp4)
So, next would do:
- PID remote-set
- target-pos remote-set
- tuning attempts
- auto-tune ?
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment