Commit cb7f289a authored by Jake Read's avatar Jake Read
Browse files

basic wrap works

parent d8f98081
......@@ -104,10 +104,10 @@ anglePlot.setHoldCount(500)
//tempPlot.setYDomain(0, 100)
anglePlot.redraw()
let angleEstPlot = new AutoPlot(130, 450, 500, 210, 'angle estimate')
angleEstPlot.setHoldCount(500)
let posEstPlot = new AutoPlot(130, 450, 500, 210, 'pos estimate')
posEstPlot.setHoldCount(500)
//tempPlot.setYDomain(0, 100)
angleEstPlot.redraw()
posEstPlot.redraw()
let angleDotPlot = new AutoPlot(130, 670, 500, 210, 'angle dot')
angleDotPlot.setHoldCount(500)
......@@ -128,7 +128,7 @@ loopQueryBtn.onClick(() => {
specPlotCount++
effortPlot.pushPt([specPlotCount, spec.effort]); effortPlot.redraw()
anglePlot.pushPt([specPlotCount, spec.angle]); anglePlot.redraw()
angleEstPlot.pushPt([specPlotCount, spec.angleEstimate]); angleEstPlot.redraw()
posEstPlot.pushPt([specPlotCount, spec.posEstimate]); posEstPlot.redraw()
angleDotPlot.pushPt([specPlotCount, spec.angleDot]); angleDotPlot.redraw()
setTimeout(loop, 10)
}).catch((err) => {
......
......@@ -108,7 +108,7 @@ export default function ClStepVM(osap, route) {
let result = {
encoder: TS.read('uint16', data, 0, true),
angle: TS.read('float32', data, 2, true),
angleEstimate: TS.read('float32', data, 6, true),
posEstimate: TS.read('float32', data, 6, true),
angleDot: TS.read('float32', data, 10, true),
effort: TS.read('float32', data, 14, true)
}
......
......@@ -114,7 +114,7 @@ void Step_CL::print_table(void){
// targets,
volatile float _tc = 0.0F; // torque output
volatile float _ta = 180.0F; // angular target
volatile float _tp = 180.0F; // position target
// set twerks
// tc: -1 : 1
......@@ -136,13 +136,13 @@ 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;
void Step_CL::set_pos_target(float tp){
clamp(&tp, 0.0F, 360.0F);
_tp = tp;
}
float Step_CL::get_angle_target(void){
return _ta;
float Step_CL::get_pos_target(void){
return _tp;
}
// PIDs
......@@ -160,12 +160,12 @@ void Step_CL::set_pid_values(float p, float i, float ilim, float d){
dTerm = d;
}
volatile float a_alpha = 0.2F; // trust in measurement
volatile float a_dot_alpha = 0.05F;
volatile float p_alpha = 0.2F; // trust in measurement
volatile float p_dot_alpha = 0.05F;
void Step_CL::set_alpha_values(float a, float adot){
a_alpha = a;
a_dot_alpha = adot;
p_alpha = a;
p_dot_alpha = adot;
}
// the control loop
......@@ -182,11 +182,15 @@ void Step_CL::run_torque_loop(void){
#define TICKS_PER_SEC 50000.0F
#define SECS_PER_TICK 1.0F / TICKS_PER_SEC
volatile uint16_t enc_reading = 0;
volatile float a_reading = 0.0F;
volatile float last_measurement = 0.0F;
volatile float a_est = 0.0F;
volatile float a_last_est = 0.0F;
// encoder / revoutions-wise,
volatile uint16_t enc_reading = 0; // 14 bit encoder reading
volatile float a_reading = 0.0F; // fp angular measurement, given lut
volatile float a_reading_last = 0.0F; // last-loop's angular measurement
volatile int32_t revolutions = 0; // count of revolutions,
// linear or rotary position wise,
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 integral = 0.0f;
......@@ -202,11 +206,11 @@ float Step_CL::get_last_pos_reading(void){
}
float Step_CL::get_last_pos_est(void){
return a_est;
return p_est;
}
float Step_CL::get_last_pos_dot(void){
return a_dot;
return (float)revolutions;
}
float Step_CL::get_output_effort(void){
......@@ -218,10 +222,25 @@ void ENC_AS5047::on_read_complete(uint16_t result){
// stash this,
enc_reading = result;
// stash old measurement, pull new from LUT
last_measurement = a_reading;
a_reading_last = a_reading;
a_reading = lut[result * 2];
// make estimate
a_est = (a_reading * a_alpha) + (a_last_est * (1 - a_alpha));
// check for wraps around the circle,
// i.e. if we went from 359 -> 1 degs, we have gone around once,
// if we went from 1 -> 359, we have decrimented one rev
if(a_reading - a_reading_last > 180.0F){
revolutions --;
} else if (a_reading_last - a_reading > 180.0F){
revolutions ++;
}
// now we have a real position measurement:
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));
// 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);
......@@ -236,9 +255,10 @@ void ENC_AS5047::on_read_complete(uint16_t result){
} else if (integral < (-iLimit)){ // as we end up doing like 10000 * 0.0001 * err ... idk
integral = -iLimit;
}
*/
// calculate output,
float output = pTerm * (a_est - _ta) + dTerm * a_dot + integral;
float output = pTerm * (p_est - _tp); //+ dTerm * a_dot + integral;
effort = output;
clamp(&output, -1.0F, 1.0F);
// _tc (nominally torque output / effort) is a local variable to this file scope,
......
......@@ -33,8 +33,8 @@ class Step_CL {
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);
void set_pos_target(float ta);
float get_pos_target(void);
float get_output_effort(void);
// pid set
void set_pid_values(float p, float i, float ilim, float d);
......
......@@ -66,8 +66,8 @@ boolean onPositionTargetPut(uint8_t* data, uint16_t len);
Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut);
boolean onPositionTargetPut(uint8_t* data, uint16_t len){
chunk_float32 tac = { .bytes = { data[0], data[1], data[2], data[3] }};
step_cl->set_angle_target(tac.f);
chunk_float32 tpc = { .bytes = { data[0], data[1], data[2], data[3] }};
step_cl->set_pos_target(tpc.f);
return true;
}
......
......@@ -1321,6 +1321,8 @@ D -0.0003
A 0.2
A* 0.05
at 10kHz
```
OK, I am satisfied with these for the time being, time now is to apply it to a real system. I have also a few things I want to know:
......@@ -1352,4 +1354,17 @@ So, then, next is having target set-points that are not directly related to the
So! There must be an established way to do this...
- wrap around 360, test with 'targ 360' key
- supervisor for offsets / etc ?
\ No newline at end of file
- supervisor for offsets / etc ?
Wondering if this is going to get tricky.
- derivative should also 'wrap' - otherwise there will be a spike between 0.0 and 360.0.
Competing approaches would be
- wrap around 360 in the subsystem, supervisor points to angles around 360
- or, count angle and *revolutions (integer, 32bit)* and track current-pos of these... translate between floating point *target* pos and *revs + angle* with units per step scalar
I think the second manner is actually best, so I'm giving that a shot. First step I take is to translate from rotary measurements -> linear / unrolled-rotary position, wrapping around & incrementing a 'revolutions' count, then I calculate 'real position' with revs * angular_pos.
Just taking a minute to figure out the particulars on this one... `a r i t h m e t i c`
\ 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