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

up to PID runtime

parent 0bba9053
......@@ -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
......
......@@ -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) })
......
......@@ -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
}
......
......@@ -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;
......
......@@ -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
......
......@@ -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.
![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
### 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.
![d-term](2021-02-15_angle-dot.png)
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