diff --git a/controller/client/clStepClient.js b/controller/client/clStepClient.js index 1e994e4104f8075dae90486592b2a22788295f6e..85aad4ebc57141852b8c1f0f499ba00b341322f4 100644 --- a/controller/client/clStepClient.js +++ b/controller/client/clStepClient.js @@ -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) => { diff --git a/controller/client/clStepVM.js b/controller/client/clStepVM.js index 55a20658808dffef9e99d107332f776c9bac8f69..a9334d9c9076c4a3245ef7fde8400c24d341be1e 100644 --- a/controller/client/clStepVM.js +++ b/controller/client/clStepVM.js @@ -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) } diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp index b05ac2652326d00393a2926ecf917eeebfd0288d..8dca1319bccd18705ec1da52e3a13ad879893e7e 100644 --- a/firmware/cl-step-controller/src/drivers/step_cl.cpp +++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp @@ -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, diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h index 13c6893ae23277f3b9e8b64750b1ceba6cb9b31e..e23546f151d5b1f8f77d6702769dd0064686ccb3 100644 --- a/firmware/cl-step-controller/src/drivers/step_cl.h +++ b/firmware/cl-step-controller/src/drivers/step_cl.h @@ -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); diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp index 2cacee8d2e46a215382e189da5335f7416e458f6..27afad4b3e423641330bad7ab64ea45deefaab50 100644 --- a/firmware/cl-step-controller/src/main.cpp +++ b/firmware/cl-step-controller/src/main.cpp @@ -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; } diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md index 9d68d21f15c5bda447454ec4f77131069359ac45..c552a6ed4a88db84eeab7f53cee38d4d5c751ebb 100644 --- a/log/cl-step-control-log.md +++ b/log/cl-step-control-log.md @@ -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