diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c16a69..a2967bc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,19 @@ This project did not use formal release tags through most of its history, so thi ### Added +- **SET_SPEED_FF sets the speed loop's feedforward** (`DCMT_OP_SET_SPEED_FF`, + `DCMT_CAP_SPEED_FEEDFORWARD`, bread-crumbs-contracts#75). Per motor: the + static PWM, the output RPM per PWM step above it (x1000) and the bound on + the PI trim. The firmware stores them and applies them on the next + speed-mode pass, converting RPM per PWM to DCMotorTacho's counts/s per + PWM with each motor's CPR. Until a controller sends it the loop is a + plain PI (no feedforward, trim 255, no bound). A payload that isn't 8 + bytes is refused `MALFORMED`, and a build without the speed loop refuses + it `NOT_BUILT`; the capability bit is advertised only where the speed + loop is built. Serial `SPEEDFF=static1,rpp1,trim1,static2,rpp2,trim2` + does the same, with `rpp` in RPM per PWM step. `firmware/README.md` + describes calibrating it from an open-loop PWM sweep. + - **`docs/hardware-revisions.md` records the motors' PWM frequencies** (#68). On the Nano, motor 1 (D6, Timer0) switches at 976.6 Hz and motor 2 (D10, Timer1) at 490.2 Hz; on the Nano Every both are @@ -301,6 +314,29 @@ This project did not use formal release tags through most of its history, so thi ### Changed +- **Breaking: the speed loop drives PWM directly (module major 3).** + Requires DCMotorServo `^2.0.0` (feastorg/dcmotorservo#26) and + bread-crumbs-contracts `^0.13.0` (`firmware/platformio.ini` and + `slice.yaml`). The speed loop is now feedforward (SET_SPEED_FF) plus a PI + trim on the measured speed, instead of a PID moving the position target. + `DCMT_MODULE_VER_MAJOR` is 3, from the contracts, so a controller built + for major 2 refuses this firmware rather than sending gains in the old + units. SET_PID's speed gains, and serial `PIDSPEED=`, are now Kp in PWM + per RPM and Ki in PWM per RPM-second; Kd is accepted and stored but + unused. The defaults are Kp 0.6, Ki 3.0, Kd 0 (were 0.15, 0.2, 0.01 in + the old units). In the host motor model with the model's feedforward and + trim 20, 60 RPM is within 5 % 210 ms after the command, from 1.29 s on + `main`; after a 0.9 s hold the peak 1 s speed is 60.8 RPM, from 61.8. + At CPR 848 the old loop drove backwards for one sample on entering speed + mode after some open-loop reverse timings (15 of 101 timings from 550 to + 750 ms); with this loop none does (`test_speed_entry_62 sweep`). Host + model only; not yet run on a board. +- **The encoder counts per turn default to 848** (#103), from 798. + `MOTOR1_CPR` and `MOTOR2_CPR` are the E-S Motor 36PG-3429-19-EN's + measured counts per output turn + ([record](docs/bench/2026-11-02-bus-lockup-and-cpr.md)); at 798 every + RPM read and commanded was about 6 % high. Another motor sets its own + with `-DMOTOR1_CPR=` / `-DMOTOR2_CPR=`. - Requires bread-crumbs-contracts `^0.12.0` (`firmware/platformio.ini` and `slice.yaml`), the release that defines GET_INPUTS and `DCMT_CAP_INPUTS`. It is additive: the four default builds are @@ -404,6 +440,18 @@ This project did not use formal release tags through most of its history, so thi is incremented on the released->pressed transition in `processEStop()`, mirroring `wdTripCount`. +### Removed + +- **The speed loop's lead cap and the integral freeze** (#63, #89). + DCMotorServo 2.0 has no position target to cap, so `setMaxLead()` and + `DCMT_TACHO_MAX_LEAD` are gone, and its conditional integration replaces + the firmware's stopgap that ran the speed PID with Ki 0 while the cap + clamped (`lead_capped()`, `speed_tunings()`). The trim bound from + SET_SPEED_FF now limits what a held shaft banks. Stall detection (#64) is + unchanged. Their host tests, `test_max_lead` and `test_speed_freeze`, go + with them; `test_stall_release_89` still checks for no lunge after a + short hold. + ### Fixed - **A build without `DCMT_HW_GEN` now fails instead of building gen1** (#17). diff --git a/firmware/README.md b/firmware/README.md index 250c235..9af95ad 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -89,22 +89,27 @@ terminal reaches which pin is in `docs/hardware-revisions.md`. The runtime uses: - `DCMotorServo` for closed-loop position control -- `DCMotorTacho` for cascaded closed-loop speed control +- `DCMotorTacho` (DCMotorServo 2.x) for closed-loop speed control: a PI on + PWM around a per-motor feedforward (see Speed Loop) These defaults are a starting point for one motor, not properties of the -firmware. They come from a single bench motor: the gains and servo values -are the archive implementation's, and the speed Ki was chosen against a -first-order model fitted to that motor (#90; the model lives in the host -motor tests, `firmware/test/host/motor_sim.h`). Tune them for each motor: -the PID gains at runtime with `SET_PID` (or serial `PIDPOS=` / -`PIDSPEED=`), and every value per build with a flag such as +firmware. They come from a single bench motor: the position gains and servo +values are the archive implementation's, and the speed gains were checked +against a first-order model fitted to that motor (#90; the model lives in +the host motor tests, `firmware/test/host/motor_sim.h`). Tune them for each +motor: the gains at runtime with `SET_PID` (or serial `PIDPOS=` / +`PIDSPEED=`), the speed feedforward with `SET_SPEED_FF` (or serial +`SPEEDFF=`), and every value per build with a flag such as `-DDCMT_SPEED_PID_KI=` (each is `#ifndef`-guarded in `include/config_hardware.h`). -- `MOTOR1_CPR = 798` -- `MOTOR2_CPR = 798` +- `MOTOR1_CPR = 848` +- `MOTOR2_CPR = 848` (measured on the bench, #103) - outer (position) PID defaults: `0.15 / 0.01 / 0.01` -- inner (speed) PID defaults: `0.15 / 0.2 / 0.01` +- speed PI defaults: Kp `0.6` PWM per RPM, Ki `3.0` PWM per RPM-second + (Kd `0.0`, accepted and unused) +- speed feedforward default: none (static 0, RPM per PWM 0, trim 255): a + plain PI until a controller sends `SET_SPEED_FF` - servo PWM skip: `15` - servo max PWM: `150` - servo accuracy: `10` @@ -113,11 +118,71 @@ the PID gains at runtime with `SET_PID` (or serial `PIDPOS=` / Speed commands/capability are only available when `DCMT_ENABLE_SPEED_LOOP=1`. +## Speed Loop + +Module major 3 (bread-crumbs-contracts 0.13.0, DCMotorServo 2.0.0). Every +speed interval (`DCMT_TACHO_INTERVAL_MS`) each motor is driven at + +```text +pwm = feedforward + trim +feedforward = sign(target) * static + target / rpm_per_pwm +trim = Kp * error + integral, bounded to +-trim +``` + +with error the target RPM less the measured RPM, and the result bounded to +the servo max PWM. The integral doesn't accumulate while the trim or the +output is at its bound in the error's direction, so a held shaft banks at +most `trim` of drive and doesn't lunge on release (#89). Stall detection +(see Stall Faults) still latches a shaft held for the stall timeout. + +- **Gains:** `SET_PID` with `DCMT_PID_LOOP_SPEED`, or serial + `PIDSPEED=kp,ki,kd`. Kp is PWM per RPM of speed error and Ki PWM per + RPM-second. Kd is accepted and stored, and unused: the loop is a PI. Gains + from module major 2 firmware (DCMotorServo 1.x) don't carry over; they + were in the old loop's units. +- **Feedforward:** `SET_SPEED_FF` (`DCMT_OP_SET_SPEED_FF`, advertised as + `DCMT_CAP_SPEED_FEEDFORWARD`, bit 13, only where the speed loop is built), + 8 bytes `[static1:u8][rpm_per_pwm1:u16][trim1:u8][static2:u8][rpm_per_pwm2:u16][trim2:u8]`: + - `static`: PWM that just starts the shaft turning, 0 to 255; + - `rpm_per_pwm`: output RPM per PWM step above `static`, x1000 (0 to + 65.535); 0 is no speed term. The firmware converts it to the library's + counts/s per PWM step with the motor's CPR; + - `trim`: the bound on the PI trim, PWM; 255 is no bound. About 20 keeps + a held shaft from banking more than that. + + Stored per motor and applied on the next speed-mode pass. A payload that + isn't 8 bytes is refused `MALFORMED`, and a build without the speed loop + refuses it `NOT_BUILT`; either stores nothing. Like `SET_PID`, it is a + configuration write and is taken during a watchdog trip, e-stop or + fault. There is no readback. +- **Serial:** `SPEEDFF=static1,rpp1,trim1,static2,rpp2,trim2`, the same six + fields in the same order, with `rpp` in RPM per PWM step as a decimal + (`SPEEDFF=10,1.719,20,10,1.719,20`). Only where the speed loop is built. + +### Calibrating the feedforward + +The values come from the motor and its load, so there are no useful +defaults. In open loop, sweep the PWM in steps (`SET_OPEN_LOOP` or +`M1PWM=`) and at each step read the steady speed from GET_STATE (or `READ`) +once it settles. Fit a line, RPM = `rpm_per_pwm` * (PWM - `static`), over +the PWMs where the shaft turns: `rpm_per_pwm` is the slope and `static` +where the line crosses 0 RPM (the intercept on the PWM axis). Err towards +less drive: a lower `static` and a higher `rpm_per_pwm`. Feedforward that +under-drives costs a little start-up time, which the trim closes, while +over-drive overshoots at start-up. If the sweep is done in both directions, +take the lower `static` and the higher `rpm_per_pwm` of the two. + +In the host motor model (24.3 counts/s per PWM above a deadband of 10 PWM, +at CPR 848) that is `static` 10 and `rpm_per_pwm` 24.3 * 60 / 848 = 1.719, +and with trim 20 and the default gains 60 RPM is within 5 % 210 ms after +the command (`test_speed_baseline_90`). Not yet checked on a board. + ## Capability Mapping (`GET_CAPS`) - Level 1: baseline (open loop and brakes); the `_noenc` and `_analog1` envs - Level 2: baseline + closed-position + PID tuning -- Level 3: Level 2 + closed-speed +- Level 3: Level 2 + closed-speed, with `DCMT_CAP_SPEED_FEEDFORWARD` + (`SET_SPEED_FF`) Controllers should gate behavior by capability flags, not by generation labels. @@ -155,12 +220,12 @@ bread-crumbs-contracts `bread_refuse.h`: | Reason | When | | --- | --- | -| `BREAD_REFUSE_MALFORMED` | the payload does not unpack; SET_PID is not 13 bytes; SET_SETPOINT is not 4 or 5 bytes; SET_WATCHDOG lacks its u16; CLEAR_WATCHDOG_TRIP or CLEAR_FAULTS carries a payload | +| `BREAD_REFUSE_MALFORMED` | the payload does not unpack; SET_PID is not 13 bytes; SET_SPEED_FF is not 8 bytes; SET_SETPOINT is not 4 or 5 bytes; SET_WATCHDOG lacks its u16; CLEAR_WATCHDOG_TRIP or CLEAR_FAULTS carries a payload | | `BREAD_REFUSE_ESTOP` | SET_OPEN_LOOP, SET_MODE or SET_SETPOINT while the hardware e-stop is held, or a SET_BRAKE that asks for a release then; this reason wins when a watchdog trip is held too | | `BREAD_REFUSE_WATCHDOG_TRIP` | SET_OPEN_LOOP, SET_MODE or SET_SETPOINT while a trip is held, or a SET_BRAKE that asks for a release then; this reason wins over a latched fault | | `BREAD_REFUSE_FAULT` | SET_OPEN_LOOP, SET_MODE or SET_SETPOINT while a stall fault is latched, or a SET_BRAKE that asks for a release then | | `BREAD_REFUSE_UNDEFINED_VALUE` | a SET_MODE, SET_PID or SET_SETPOINT loop the contract does not define; a SET_WATCHDOG flag other than `BREAD_WATCHDOG_FLAG_COMMANDS_ONLY`; a SET_OPEN_LOOP PWM outside `-DCMT_PWM_MAX` to `DCMT_PWM_MAX` (255) | -| `BREAD_REFUSE_NOT_BUILT` | closed-loop speed (SET_MODE, or a SET_PID or SET_SETPOINT loop SPEED) when the speed loop is not built | +| `BREAD_REFUSE_NOT_BUILT` | closed-loop speed (SET_MODE, or a SET_PID or SET_SETPOINT loop SPEED) and SET_SPEED_FF when the speed loop is not built | | `BREAD_REFUSE_WRONG_MODE` | SET_OPEN_LOOP outside open loop | | `BREAD_REFUSE_UNKNOWN_OP` | an opcode with no registered command handler, including a GET opcode sent as a command frame instead of through SET_REPLY | @@ -213,8 +278,9 @@ not stored: `SET_OPEN_LOOP`, `SET_SETPOINT`, `SET_MODE` and a `SET_BRAKE` that would release a brake, and over serial `MODE=`, `M1PWM=`/`M2PWM=`, `M1POS=`/`M2POS=`, `M1SPEED=`/`M2SPEED=`, `BRAKE1=0` and `BRAKE2=0`. The I2C ones are refused with `BREAD_REFUSE_WATCHDOG_TRIP`, so GET_ACK says -so (see Refused Commands). Engaging a brake, PID tuning (`SET_PID`, -`PIDPOS=`, `PIDSPEED=`), `SET_WATCHDOG`/`WDOG=` and `READ` still work. +so (see Refused Commands). Engaging a brake, PID and feedforward tuning (`SET_PID`, `SET_SPEED_FF`, +`PIDPOS=`, `PIDSPEED=`, `SPEEDFF=`), `SET_WATCHDOG`/`WDOG=` and `READ` still +work. Clearing a trip resumes nothing; the brakes stay engaged and no motor moves. To resume: diff --git a/firmware/include/config_hardware.h b/firmware/include/config_hardware.h index bcdc3a9..0484631 100644 --- a/firmware/include/config_hardware.h +++ b/firmware/include/config_hardware.h @@ -38,10 +38,10 @@ #define SERIAL_UPDATE_TIME_MS 1000 // Closed-loop tuning defaults. These, and the other motor-specific values -// below (counts/rev, PWM skip and cap, accuracy, the tacho interval and lead -// cap, the stall thresholds), come from a single bench motor: the PID gains -// and servo values are the archive implementation's, and the speed Ki was -// chosen against a first-order model fitted to that motor (#90, the host +// below (counts/rev, PWM skip and cap, accuracy, the tacho interval, the +// stall thresholds), come from a single bench motor: the position gains and +// servo values are the archive implementation's, and the speed gains were +// checked against a first-order model fitted to that motor (#90, the host // motor tests). They are a starting point, not a property of the firmware: // tune them per motor at runtime with SET_PID / PIDPOS= / PIDSPEED=, or per // build with flags, e.g. -DDCMT_SPEED_PID_KI=. @@ -55,17 +55,18 @@ #define DCMT_POS_PID_KD 0.01f #endif +// Speed gains (module major 3, DCMotorServo 2.x): the speed loop is a PI on +// PWM around the feedforward SET_SPEED_FF sets. KP is PWM per RPM of speed +// error, KI PWM per RPM-second; KD is accepted and stored but not used. +// These are DCMotorTacho's own defaults. #ifndef DCMT_SPEED_PID_KP -#define DCMT_SPEED_PID_KP 0.15f +#define DCMT_SPEED_PID_KP 0.6f #endif -// 0.01 left the speed loop P-only for seconds: 60 RPM settled near 69 % of -// the target and took about 20 s to close the rest. 0.2 reaches it within -// 5 % in about 1.3 s in the model (#90). #ifndef DCMT_SPEED_PID_KI -#define DCMT_SPEED_PID_KI 0.2f +#define DCMT_SPEED_PID_KI 3.0f #endif #ifndef DCMT_SPEED_PID_KD -#define DCMT_SPEED_PID_KD 0.01f +#define DCMT_SPEED_PID_KD 0.0f #endif #ifndef DCMT_SERVO_PWM_SKIP @@ -83,13 +84,6 @@ #define DCMT_TACHO_INTERVAL_MS 5 #endif -// How far, in encoder counts, the speed loop may drive the position target -// ahead of the encoder. Without a cap a held shaft banks travel and lunges -// on release; 300 removed that on the 2026-10-22 bench (#63). -#ifndef DCMT_TACHO_MAX_LEAD -#define DCMT_TACHO_MAX_LEAD 300 -#endif - // Stall faults (#64, DCMT_CAP_FAULTS). In closed loop, a motor driven for // DCMT_STALL_TIMEOUT_MS without its encoder moving DCMT_STALL_MIN_COUNTS // counts latches DCMT_FAULT_M1_STALL or _M2_STALL, which brakes both motors @@ -171,12 +165,17 @@ #error "MOTOR2_PWM_PIN has no hardware timer on this MCU -- analogWrite would degrade to on/off. See docs/hardware-revisions.md" #endif -// Encoder counts-per-rev (archive-proven closed-loop value). +// Encoder counts per output-shaft revolution. 848 is the E-S Motor +// 36PG-3429-19-EN measured on the bench on 2026-11-02 (#103): -16 008 +// counts left a flag on its output shaft 1/8 turn from where it started, +// 18.875 turns (about +-1.2 counts). The datasheet's 12 PPR x 4 x 1:19 would +// be 912, and the earlier 798 read speeds about 6 % high. Another motor sets +// its own with -DMOTOR1_CPR= / -DMOTOR2_CPR=. #ifndef MOTOR1_CPR -#define MOTOR1_CPR 798 +#define MOTOR1_CPR 848 #endif #ifndef MOTOR2_CPR -#define MOTOR2_CPR 798 +#define MOTOR2_CPR 848 #endif #endif // HARDWARE_CONFIG_H diff --git a/firmware/include/globals.h b/firmware/include/globals.h index e195912..5f575cb 100644 --- a/firmware/include/globals.h +++ b/firmware/include/globals.h @@ -25,6 +25,17 @@ struct PIDTunings PIDTunings(float p, float i, float d) : kp(p), ki(i), kd(d) {} }; +// One motor's speed-loop feedforward, as SET_SPEED_FF carries it: static +// PWM, output RPM per PWM step above static x1000 (0 = no speed term), and +// the bound on the PI trim in PWM (DCMT_SPEED_FF_TRIM_UNBOUNDED = none). +// The defaults are no feedforward and no bound: a plain PI on PWM. +struct SpeedFeedforward +{ + uint8_t pwmStatic = 0; + uint16_t rpmPerPwmX1000 = 0; + uint8_t trim = 255; +}; + // ---- Shared state structs ---- struct DCMT_SLICE { @@ -47,6 +58,10 @@ struct DCMT_SLICE PIDTunings posPid2 = {DCMT_POS_PID_KP, DCMT_POS_PID_KI, DCMT_POS_PID_KD}; PIDTunings speedPid1 = {DCMT_SPEED_PID_KP, DCMT_SPEED_PID_KI, DCMT_SPEED_PID_KD}; PIDTunings speedPid2 = {DCMT_SPEED_PID_KP, DCMT_SPEED_PID_KI, DCMT_SPEED_PID_KD}; +#if DCMT_ENABLE_SPEED_LOOP + SpeedFeedforward speedFf1; + SpeedFeedforward speedFf2; +#endif }; struct Timing @@ -110,6 +125,7 @@ void handler_set_brake(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *dat void handler_set_mode(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); void handler_set_setpoint(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); void handler_set_pid(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); +void handler_set_speed_ff(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); void handler_set_watchdog(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); void handler_clear_watchdog_trip(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); void handler_clear_faults(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data); diff --git a/firmware/platformio.ini b/firmware/platformio.ini index ed492a4..d6a2d91 100644 --- a/firmware/platformio.ini +++ b/firmware/platformio.ini @@ -11,9 +11,9 @@ upload_speed = 115200 monitor_speed = 115200 lib_deps = cameronbrooks11/CRUMBS @ ^0.17.0 - cameronbrooks11/bread-crumbs-contracts @ ^0.12.0 + cameronbrooks11/bread-crumbs-contracts @ ^0.13.0 cameronbrooks11/LMD18200 @ ^0.1.5 - cameronbrooks11/DCMotorServo @ ^1.2.1 + cameronbrooks11/DCMotorServo @ ^2.0.0 paulstoffregen/Encoder @ ^1.4.4 cameronbrooks11/bread-slice-core @ ^0.2.0 build_flags = diff --git a/firmware/src/dcmt_handlers.cpp b/firmware/src/dcmt_handlers.cpp index 92f8f07..d0707ed 100644 --- a/firmware/src/dcmt_handlers.cpp +++ b/firmware/src/dcmt_handlers.cpp @@ -286,6 +286,8 @@ void handler_set_pid(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, #if DCMT_ENABLE_SPEED_LOOP else if (v.loop == DCMT_PID_LOOP_SPEED) { + // Module major 3: kp PWM per RPM, ki PWM per RPM-second. kd is + // stored but the speed loop (a PI) doesn't use it. slice.speedPid1 = {kp1, ki1, kd1}; slice.speedPid2 = {kp2, ki2, kd2}; } @@ -309,6 +311,35 @@ void handler_set_pid(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, #endif } +// SET_SPEED_FF (DCMT_CAP_SPEED_FEEDFORWARD): the speed loop's per-motor +// feedforward and trim bound, as SET_PID is its gains. Like SET_PID it is a +// configuration write, taken during a hold: a malformed payload is refused +// MALFORMED, a build without the speed loop refuses it NOT_BUILT, and +// either stores nothing. Every value a field can hold is defined, so there +// is no UNDEFINED_VALUE. The loop applies it on the next speed-mode pass. +void handler_set_speed_ff(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data) +{ + dcmt_set_speed_ff_t v; + (void)opcode; + (void)user_data; + + if (dcmt_parse_set_speed_ff_payload(data, data_len, &v) != 0) + { + (void)crumbs_refuse(ctx, BREAD_REFUSE_MALFORMED); + return; + } +#if DCMT_ENABLE_SPEED_LOOP + slice.speedFf1.pwmStatic = v.static1; + slice.speedFf1.rpmPerPwmX1000 = v.rpm_per_pwm1_x1000; + slice.speedFf1.trim = v.trim1; + slice.speedFf2.pwmStatic = v.static2; + slice.speedFf2.rpmPerPwmX1000 = v.rpm_per_pwm2_x1000; + slice.speedFf2.trim = v.trim2; +#else + (void)crumbs_refuse(ctx, BREAD_REFUSE_NOT_BUILT); +#endif +} + void handler_set_watchdog(crumbs_context_t *ctx, uint8_t opcode, const uint8_t *data, uint8_t data_len, void *user_data) { uint16_t timeout_ms = 0; @@ -479,7 +510,7 @@ void reply_get_caps(crumbs_context_t *ctx, crumbs_message_t *reply, void *user_d #if DCMT_ENABLE_SPEED_LOOP level = DCMT_CAP_LEVEL_3; - flags |= DCMT_CAP_CLOSED_LOOP_SPEED; + flags |= DCMT_CAP_CLOSED_LOOP_SPEED | DCMT_CAP_SPEED_FEEDFORWARD; #endif #if DCMT_HAS_ANALOG_INPUTS flags |= DCMT_CAP_INPUTS; diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 2c26652..c96c3a6 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -86,6 +86,9 @@ static bool positionPidApplied = false; static PIDTunings appliedSpeedPid1; static PIDTunings appliedSpeedPid2; static bool speedPidApplied = false; +static SpeedFeedforward appliedSpeedFf1; +static SpeedFeedforward appliedSpeedFf2; +static bool speedFfApplied = false; #endif static int16_t clamp_pwm(long v) @@ -233,47 +236,58 @@ static void apply_position_pid_if_needed(const DCMT_SLICE &s) } #if DCMT_ENABLE_SPEED_LOOP -// True while DCMotorTacho's lead cap is clamping: the position target sits -// DCMT_TACHO_MAX_LEAD or more counts from the encoder, so the speed PID's -// output no longer moves the shaft. -static bool lead_capped(DCMotorServo &servo) +// The speed loop is DCMotorTacho 2.x's PI on PWM: kp PWM per RPM, ki PWM per +// RPM-second. kd is stored (SET_PID, PIDSPEED=) but not used. The library +// stops integrating while the trim or the output is at its limit, so a held +// shaft can't wind the integral up (#89). +static void apply_speed_pid_if_needed(const DCMT_SLICE &s) { - const long lead = servo.getRequestedPosition() - servo.getActualPosition(); - return lead >= DCMT_TACHO_MAX_LEAD || lead <= -DCMT_TACHO_MAX_LEAD; + if (speedPidApplied && + pid_tunings_equal(appliedSpeedPid1, s.speedPid1) && + pid_tunings_equal(appliedSpeedPid2, s.speedPid2)) + { + return; + } + + tacho1.setPIDTunings(s.speedPid1.kp, s.speedPid1.ki); + tacho2.setPIDTunings(s.speedPid2.kp, s.speedPid2.ki); + + appliedSpeedPid1 = s.speedPid1; + appliedSpeedPid2 = s.speedPid2; + speedPidApplied = true; } -// The speed gains in force: the runtime ones (SET_PID, serial), with Ki 0 -// while the motor's lead cap is clamping, so a held or stalled shaft doesn't -// wind up the integral and lunge on release (#89). PID_v1 doesn't rescale -// its integral when Ki changes, so the integral is frozen, not reset. -// -// Conditional integration belongs in DCMotorTacho (feastorg/dcmotorservo); -// this is a stopgap until it lands there. -static PIDTunings speed_tunings(const PIDTunings &t, bool freeze) +static bool speed_ff_equal(const SpeedFeedforward &a, const SpeedFeedforward &b) { - PIDTunings r = t; - if (freeze) - r.ki = 0.0f; - return r; + return a.pwmStatic == b.pwmStatic && a.rpmPerPwmX1000 == b.rpmPerPwmX1000 && a.trim == b.trim; } -static void apply_speed_pid_if_needed(const DCMT_SLICE &s, bool freeze1 = false, bool freeze2 = false) +// SET_SPEED_FF carries output RPM per PWM step; DCMotorTacho takes encoder +// counts/s per PWM step, which needs the motor's CPR. 0 stays 0: no speed +// term. A trim of DCMT_SPEED_FF_TRIM_UNBOUNDED (255) is the library's own +// "no bound" default. +static void apply_speed_ff(DCMotorTacho &tacho, const SpeedFeedforward &f, double cpr) { - const PIDTunings t1 = speed_tunings(s.speedPid1, freeze1); - const PIDTunings t2 = speed_tunings(s.speedPid2, freeze2); - if (speedPidApplied && - pid_tunings_equal(appliedSpeedPid1, t1) && - pid_tunings_equal(appliedSpeedPid2, t2)) + const double kv = (f.rpmPerPwmX1000 / 1000.0) * cpr / 60.0; + tacho.setFeedforward(f.pwmStatic, kv); + tacho.setTrimLimit(f.trim); +} + +static void apply_speed_ff_if_needed(const DCMT_SLICE &s) +{ + if (speedFfApplied && + speed_ff_equal(appliedSpeedFf1, s.speedFf1) && + speed_ff_equal(appliedSpeedFf2, s.speedFf2)) { return; } - tacho1.setPIDTunings(t1.kp, t1.ki, t1.kd); - tacho2.setPIDTunings(t2.kp, t2.ki, t2.kd); + apply_speed_ff(tacho1, s.speedFf1, MOTOR1_CPR); + apply_speed_ff(tacho2, s.speedFf2, MOTOR2_CPR); - appliedSpeedPid1 = t1; - appliedSpeedPid2 = t2; - speedPidApplied = true; + appliedSpeedFf1 = s.speedFf1; + appliedSpeedFf2 = s.speedFf2; + speedFfApplied = true; } #endif @@ -533,6 +547,11 @@ void setupSlice() if (rc != 0) SLICE_DEBUG_PRINTLN(F("CRUMBS: Failed to register DCMT_OP_SET_PID")); + // On every build: one without the speed loop refuses it NOT_BUILT. + rc = crumbs_register_handler(&ctx, DCMT_OP_SET_SPEED_FF, handler_set_speed_ff, nullptr); + if (rc != 0) + SLICE_DEBUG_PRINTLN(F("CRUMBS: Failed to register DCMT_OP_SET_SPEED_FF")); + rc = crumbs_register_handler(&ctx, BREAD_OP_SET_WATCHDOG, handler_set_watchdog, nullptr); if (rc != 0) SLICE_DEBUG_PRINTLN(F("CRUMBS: Failed to register BREAD_OP_SET_WATCHDOG")); @@ -637,10 +656,9 @@ void setupDCMT() #if DCMT_ENABLE_SPEED_LOOP apply_speed_pid_if_needed(slice); + apply_speed_ff_if_needed(slice); tacho1.setSpeedRPM(0); tacho2.setSpeedRPM(0); - tacho1.setMaxLead(DCMT_TACHO_MAX_LEAD); - tacho2.setMaxLead(DCMT_TACHO_MAX_LEAD); #endif slice.motor1Position = clamp_i16(servo1.getActualPosition()); @@ -950,8 +968,11 @@ void motorControlLogic() noInterrupts(); local.speedPid1 = slice.speedPid1; local.speedPid2 = slice.speedPid2; + local.speedFf1 = slice.speedFf1; + local.speedFf2 = slice.speedFf2; interrupts(); - apply_speed_pid_if_needed(local, lead_capped(servo1), lead_capped(servo2)); + apply_speed_pid_if_needed(local); + apply_speed_ff_if_needed(local); if (local.motor1Brake) { diff --git a/firmware/src/serialCommands.cpp b/firmware/src/serialCommands.cpp index 6c84bba..5c65696 100644 --- a/firmware/src/serialCommands.cpp +++ b/firmware/src/serialCommands.cpp @@ -68,6 +68,50 @@ static bool parse_pid_triplet(const char *s, float &kp, float &ki, float &kd) } #endif +#if DCMT_ENABLE_SPEED_LOOP +// SPEEDFF=static1,rpp1,trim1,static2,rpp2,trim2: SET_SPEED_FF's six fields +// in its order. static and trim are whole PWM 0 to 255 (trim 255: no +// bound); rpp is output RPM per PWM step, 0 to 65.535 (0: no speed term), +// kept to the wire's x1000 resolution. +static bool parse_speed_ff(const char *s, SpeedFeedforward &f1, SpeedFeedforward &f2) +{ + char buf[sizeof cmdBuf]; + if (strlen(s) >= sizeof buf) + return false; + strcpy(buf, s); + char *field[6]; + uint8_t n = 0; + char *p = buf; + for (;;) + { + if (n == 6) + return false; + field[n++] = p; + char *comma = strchr(p, ','); + if (comma == nullptr) + break; + *comma = '\0'; + p = comma + 1; + } + if (n != 6) + return false; + SpeedFeedforward *f[2] = {&f1, &f2}; + for (uint8_t m = 0; m < 2; m++) + { + long pwmStatic, trim; + float rpp; + if (!bsc::parseLong(field[3 * m], 0, 255, &pwmStatic) || + !bsc::parseFloat(field[3 * m + 1], 0.0f, 65.535f, &rpp) || + !bsc::parseLong(field[3 * m + 2], 0, 255, &trim)) + return false; + f[m]->pwmStatic = (uint8_t)pwmStatic; + f[m]->rpmPerPwmX1000 = (uint16_t)(rpp * 1000.0f + 0.5f); + f[m]->trim = (uint8_t)trim; + } + return true; +} +#endif + static void strupper(char *s) { for (; *s; ++s) @@ -97,8 +141,8 @@ static void processCommand(char *cmd) // While an e-stop or a watchdog trip is held, or a motor fault is // latched, commands that could make a motor move are refused, as over // I2C (dcmt_handlers.cpp): releasing the e-stop or clearing the trip or - // the fault resumes nothing. Engaging a brake, PID tuning, WDOG=, - // WDCLEAR, FAULTCLEAR and READ stay available. + // the fault resumes nothing. Engaging a brake, PID and feedforward + // tuning, WDOG=, WDCLEAR, FAULTCLEAR and READ stay available. if ((estopInput.asserted() || commandWatchdog.tripped() || motorFaults != 0) && (starts_with_P(cmd, PSTR("MODE=")) || starts_with_P(cmd, PSTR("M1PWM=")) || starts_with_P(cmd, PSTR("M2PWM=")) || @@ -306,6 +350,21 @@ static void processCommand(char *cmd) interrupts(); Serial.println(F("Speed PID updated")); } + else if (starts_with_P(cmd, PSTR("SPEEDFF="))) + { + SpeedFeedforward f1, f2; + if (!parse_speed_ff(after_prefix_P(cmd, PSTR("SPEEDFF=")), f1, f2)) + { + Serial.println(F("SPEEDFF format: SPEEDFF=static1,rpp1,trim1,static2,rpp2,trim2; static, trim 0 to 255, rpp 0 to 65.535")); + return; + } + // SET_SPEED_FF writes these from the I2C interrupt too. + noInterrupts(); + slice.speedFf1 = f1; + slice.speedFf2 = f2; + interrupts(); + Serial.println(F("Speed feedforward updated")); + } #endif else if (strcmp_P(cmd, PSTR("BRAKE1=1")) == 0) { @@ -383,6 +442,7 @@ static void processCommand(char *cmd) #endif #if DCMT_ENABLE_SPEED_LOOP Serial.println(F("Closed-loop: MODE=POS|SPEED, M1POS=, M2POS=, M1SPEED=, M2SPEED=, PIDPOS=kp,ki,kd, PIDSPEED=kp,ki,kd")); + Serial.println(F("Speed feedforward: SPEEDFF=static1,rpp1,trim1,static2,rpp2,trim2 (rpp: RPM per PWM step)")); #endif } } diff --git a/firmware/test/host/Makefile b/firmware/test/host/Makefile index 22087eb..2e5892c 100644 --- a/firmware/test/host/Makefile +++ b/firmware/test/host/Makefile @@ -66,9 +66,9 @@ CRUMBS_SRC := $(CRUMBS)/core/crumbs_core.c $(CRUMBS)/crc/crumbs_crc.c $(CRUMBS)/ CRUMBS_OBJ := $(OUT)/crumbs_core.o $(OUT)/crumbs_crc.o $(OUT)/crc8_nibble.o # test_status_led runs once per e-stop case (the test target); the others # run once. -SINGLE := test_dispatch test_mode_pwm test_applied_pwm test_serial_burst test_max_lead test_estop_hold test_watchdog_trip test_serial_numbers test_setpoint_loop test_watchdog_lease test_stall_fault test_hold_brakes test_speed_freeze test_status_led_hold +SINGLE := test_dispatch test_mode_pwm test_applied_pwm test_serial_burst test_speed_ff test_estop_hold test_watchdog_trip test_serial_numbers test_setpoint_loop test_watchdog_lease test_stall_fault test_hold_brakes test_status_led_hold TESTS := $(SINGLE) test_status_led -NOENC_SINGLE := test_noenc test_dispatch test_serial_burst test_estop_hold test_watchdog_trip test_watchdog_lease test_hold_brakes test_status_led_hold +NOENC_SINGLE := test_noenc test_dispatch test_speed_ff test_serial_burst test_estop_hold test_watchdog_trip test_watchdog_lease test_hold_brakes test_status_led_hold NOENC_TESTS := $(NOENC_SINGLE) test_status_led ANALOG_SINGLE := test_analog $(filter-out test_noenc,$(NOENC_SINGLE)) ANALOG_TESTS := $(ANALOG_SINGLE) test_status_led @@ -179,7 +179,7 @@ motor: $(MOTOR_BINS) for t in $(MOTOR_SINGLE); do \ echo "== $$e: $$t"; $(OUT)/$$e/$$t || status=1; \ done; \ - for c in reverse forward; do \ + for c in reverse forward sweep; do \ echo "== $$e: test_speed_entry_62 $$c"; $(OUT)/$$e/test_speed_entry_62 $$c || status=1; \ done; \ for c in speed_held position_rest position_held speed_10; do \ diff --git a/firmware/test/host/README.md b/firmware/test/host/README.md index 648a40f..1fe1eb7 100644 --- a/firmware/test/host/README.md +++ b/firmware/test/host/README.md @@ -19,7 +19,8 @@ and run on the host. Nothing is reimplemented: The motor tests (`make motor`) build the same firmware with the real DCMotorServo, DCMotorTacho and PID_v1 from libdeps in place of the two motor-library stubs, so the closed loop runs as it does on the board, with the -firmware's own gains, PWM limits, speed interval and lead cap. `motor_sim.cpp` +firmware's own PWM limits and speed interval, and the speed gains and +feedforward `sim_boot()` sends over I2C for the model motor (`motor_sim.h`). `motor_sim.cpp` models a motor on each channel between the LMD18200 and Encoder stubs. Frames go in through `crumbs_peripheral_handle_receive()` and replies come @@ -58,14 +59,13 @@ fails. | `test_applied_pwm.cpp` | GET_STATE `m1_pwm`/`m2_pwm` report the PWM each motor is driven at: open loop, braked, position and speed (the servo stub's output), and e-stop (#61) | | `test_mode_pwm.cpp` | Leaving open loop clears the open-loop PWM through serial `MODE=` and through `SET_MODE`, so GET_STATE reads 0 and a return to open loop doesn't resume the old PWM (#36) | | `test_serial_burst.cpp` | A burst of serial lines is answered one line per `loop()` pass, so an e-stop pressed mid-burst is asserted before the next line and the lines after it are refused (#49) | -| `test_max_lead.cpp` | `setup()` caps both speed loops' lead at `DCMT_TACHO_MAX_LEAD` where the speed loop is built, and sets no cap where it isn't (#63) | +| `test_speed_ff.cpp` | The speed loop's gains and feedforward (module major 3): GET_VERSION major 3; the default gains Kp 0.6 Ki 3.0, no feedforward and trim 255 at boot; SET_PID speed gains reach each tacho as (kp, ki); SET_SPEED_FF stored per motor and applied once as static, kv = rpm_per_pwm * CPR / 60 counts/s per PWM and trim (1.719 RPM per PWM at CPR 848 is 24.3), taken during an e-stop; 7 and 9 bytes refused `MALFORMED` with nothing stored; serial `SPEEDFF=` and its malformed forms; `DCMT_CAP_SPEED_FEEDFORWARD` (bit 13) advertised exactly where the speed loop is built. Where it isn't (gen1/gen2 Nano, `_noenc`, `_analog1`): SET_SPEED_FF refused `NOT_BUILT`, no `SPEEDFF=`, no bit 13 | | `test_estop_hold.cpp` | An e-stop press during open-loop drive: GET_STATE reports it and its count, both motors brake, a SET is refused `ESTOP`, the release is debounced and resumes nothing, and a second press counts (`DCMT_CAP_ESTOP_HOLD`) | | `test_watchdog_trip.cpp` | A command-watchdog trip: GET_WATCHDOG reports it, both motors brake, a SET is refused `WATCHDOG_TRIP`, a re-arm leaves it latched, and `CLEAR_WATCHDOG_TRIP` lifts it, keeps the count and resumes nothing | | `test_serial_numbers.cpp` | Serial numbers are parsed strictly: each row of #60 (trailing junk, empty, NaN, inf, out of range, wrong field count) is refused with no change, and in-range values still apply (#60) | | `test_setpoint_loop.cpp` | SET_SETPOINT names its loop: the 4-byte form writes the position setpoints only and the 5-byte form the named loop's; other lengths are refused `MALFORMED`, an undefined loop `UNDEFINED_VALUE`, and loop SPEED without the speed loop `NOT_BUILT`. Speed mode after a position target requests 0, and after a speed target that target; a speed target keeps the post-hold position follow, a position target ends it; GET_CAPS sets `DCMT_CAP_SETPOINT_LOOP` (#57) | | `test_watchdog_lease.cpp` | The commands-only lease: polls, SET_PID and serial lines no longer refresh the watchdog while SET_MODE, SET_BRAKE and a re-sent SET_WATCHDOG do; the 2-byte form and serial `WDOG=` end the lease; an undefined flag is refused and arms nothing; GET_CAPS sets `DCMT_CAP_WATCHDOG_LEASE` (bread-crumbs-contracts#63) | | `test_stall_fault.cpp` | Stall faults with the servo stub's stall latch standing in for the library's: setup() enables detection at `DCMT_STALL_TIMEOUT_MS`/`DCMT_STALL_MIN_COUNTS`; a stall latches `DCMT_FAULT_M1_STALL`/`_M2_STALL`, brakes both motors and leaves no motion pending; SET_OPEN_LOOP, SET_SETPOINT, SET_MODE and a brake release are refused `FAULT` (an e-stop's reason wins); GET_FAULTS, CLEAR_FAULTS (a payload is `MALFORMED`) and serial `FAULTCLEAR` clear and resume nothing; GET_STATE stays 20 bytes; GET_CAPS sets `DCMT_CAP_FAULTS` (#64) | -| `test_speed_freeze.cpp` | The speed loop's integral freeze (#89): while a motor's lead reaches `DCMT_TACHO_MAX_LEAD` either way, its speed PID runs with Ki 0 and the runtime Kp and Kd, including a SET_PID sent while clamped; the other motor, and the same one once the lead drops, run the runtime gains in full. Skips where the speed loop isn't built | | `test_hold_brakes.cpp` | `motorControlLogic()`'s hold: with an e-stop, a watchdog trip or a stall fault each alone, both motors are braked on every pass and no PWM is written, even with the brake flags cleared and a PWM stored | | `test_status_led.cpp` | Blue through the boot wait, then green or red on the first `loop()` pass, with the e-stop released or held from power-up; no LED driven on gen1 (#56) | | `test_status_led_hold.cpp` | Amber while a command-watchdog trip or a latched stall fault holds the motors, red over amber while the e-stop is held, amber again on the release while the hold stays latched, and green once it is cleared; no LED driven on gen1 | @@ -74,8 +74,8 @@ Motor tests (`make motor`): | File | Covers | | --- | --- | -| `test_speed_entry_62.cpp` | Closed-loop speed entered after open-loop travel starts from where the shaft is: after full reverse PWM, and after full forward PWM and a stop, speed mode at +60 RPM never drives backwards or at `DCMT_SERVO_MAX_PWM`, and the stopped shaft never moves back (#62) | -| `test_speed_baseline_90.cpp` | The #90 regression, with the default speed gains: at 60 RPM, a 200 ms mean is within 5 % of the target by 2 s after the command, speed is within 1 RPM over 9-10 s and 15-20 s, and the peak 200 ms mean overshoots by under 10 % | +| `test_speed_entry_62.cpp` | Closed-loop speed entered after open-loop travel starts from where the shaft is: after full reverse PWM, and after full forward PWM and a stop, speed mode at +60 RPM never drives backwards or at `DCMT_SERVO_MAX_PWM`, and the stopped shaft never moves back; and the reverse entry swept over 550 to 750 ms of full reverse in 2 ms steps (`sweep`, one forked child per timing), never driving backwards in the first 1 s at any of the 101 (#62) | +| `test_speed_baseline_90.cpp` | The #90 regression, with the speed loop as `sim_boot()` configures it: at 60 RPM, a 200 ms mean is within 5 % of the target by 2 s after the command, speed is within 1 RPM over 9-10 s and 15-20 s, and the peak 200 ms mean overshoots by under 10 % | | `test_stall_release_89.cpp` | The #89 regression: at 60 RPM with the shaft held for 100 ms under `DCMT_STALL_TIMEOUT_MS`, no fault latches and the peak 1 s speed after release is within 5 RPM of the target, where the wound-up integral used to lunge; a 4 s hold, #89's own, latches a stall fault and the shaft stays braked after release (#64) | | `test_speed_zero_reads_0.cpp` | GET_STATE `spd1` reads 0 once motor 1 stops at a speed setpoint of 0, after 3 s at 60 RPM; before DCMotorServo 1.2.1 it kept the last running speed (feastorg/dcmotorservo#18) | | `test_stall_fault_64.cpp` | Stall detection with the real libraries (#64), one process per case: a shaft held at 60 RPM latches `M1_STALL` `DCMT_STALL_TIMEOUT_MS` +/- 50 ms after the hold and stays braked and still after release; position mode at rest inside the deadband for 10 s never latches; a shaft held 5000 counts from its position target latches; 20 s at 10 RPM never latches | diff --git a/firmware/test/host/motor_sim.cpp b/firmware/test/host/motor_sim.cpp index f62843f..5b2563c 100644 --- a/firmware/test/host/motor_sim.cpp +++ b/firmware/test/host/motor_sim.cpp @@ -57,9 +57,34 @@ static void record() sim_trace.push_back({host_now, motor1Encoder.count, motor1AppliedPWM}); } +static void put16(uint8_t *p, uint16_t v) +{ + p[0] = (uint8_t)(v & 0xFF); + p[1] = (uint8_t)(v >> 8); +} + +static bool applied(); + void sim_boot() { setup(); + loop(); + + // The speed loop configured as a controller would for the model motor: + // gains Kp 0.6 Ki 3.0, and the feedforward an open-loop sweep of it gives. + uint8_t pid[13] = {DCMT_PID_LOOP_SPEED}; + put16(pid + 1, SIM_SPEED_KP_X1000); + put16(pid + 3, SIM_SPEED_KI_X1000); + put16(pid + 7, SIM_SPEED_KP_X1000); + put16(pid + 9, SIM_SPEED_KI_X1000); + host_send(DCMT_TYPE_ID, DCMT_OP_SET_PID, pid, 13); + host_check("SET_PID speed (Kp 0.6, Ki 3.0) is applied", applied()); + uint8_t ff[8] = {SIM_FF_STATIC, 0, 0, SIM_FF_TRIM, SIM_FF_STATIC, 0, 0, SIM_FF_TRIM}; + put16(ff + 1, SIM_FF_RPM_PER_PWM_X1000); + put16(ff + 5, SIM_FF_RPM_PER_PWM_X1000); + host_send(DCMT_TYPE_ID, DCMT_OP_SET_SPEED_FF, ff, 8); + host_check("SET_SPEED_FF (static 10, 1.719 RPM per PWM, trim 20) is applied", applied()); + loop(); record(); } diff --git a/firmware/test/host/motor_sim.h b/firmware/test/host/motor_sim.h index 89dbacc..665d281 100644 --- a/firmware/test/host/motor_sim.h +++ b/firmware/test/host/motor_sim.h @@ -38,7 +38,19 @@ struct SimSample }; extern std::vector sim_trace; -// setup(), then one loop() pass. +// The speed loop's configuration for the model motor, sent by sim_boot(): +// SET_PID speed gains Kp 0.6, Ki 3.0 (x1000), and SET_SPEED_FF with the +// model's feedforward, the values an open-loop PWM sweep of it gives: +// static 10 (the deadband) and 24.3 counts/s per PWM, which at CPR 848 is +// 24.3 * 60 / 848 = 1.719 RPM per PWM; trim 20. +#define SIM_SPEED_KP_X1000 600 +#define SIM_SPEED_KI_X1000 3000 +#define SIM_FF_STATIC 10 +#define SIM_FF_RPM_PER_PWM_X1000 1719 +#define SIM_FF_TRIM 20 + +// setup(), one loop() pass, the speed-loop configuration above for both +// motors, and one more pass. void sim_boot(); // Runs loop() for ms of fake time, stepping both motors after every pass. diff --git a/firmware/test/host/stubs/DCMotorTacho.h b/firmware/test/host/stubs/DCMotorTacho.h index e15d37e..8ba744a 100644 --- a/firmware/test/host/stubs/DCMotorTacho.h +++ b/firmware/test/host/stubs/DCMotorTacho.h @@ -1,16 +1,20 @@ -// Host stand-in for DCMotorTacho: records the requested speed, the lead cap -// and the PID gains, and run() runs its servo, as the real speed loop does. +// Host stand-in for DCMotorTacho (DCMotorServo 2.x): records the requested +// speed, the PI gains, the feedforward and the trim bound, and run() runs its +// servo, as the real speed loop drives it. #pragma once #include "DCMotorServo.h" -// The cap each motor's setMaxLead() last set, by servo index; 0 is none. -inline long host_tacho_max_lead[2] = {0, 0}; // The speed each motor's setSpeedRPM() last requested, by servo index. inline double host_tacho_requested[2] = {0, 0}; -// The gains each motor's setPIDTunings() last set (kp, ki, kd), by servo -// index, and how many times it was called. -inline double host_tacho_tunings[2][3] = {{-1, -1, -1}, {-1, -1, -1}}; +// The gains each motor's setPIDTunings() last set (kp, ki), by servo index, +// and how many times it was called. +inline double host_tacho_tunings[2][2] = {{-1, -1}, {-1, -1}}; inline unsigned host_tacho_tunings_calls[2] = {0, 0}; +// What each motor's setFeedforward() last set (pwmStatic, kv in counts/s per +// PWM step) and setTrimLimit() last set, by servo index; -1 if never. +inline double host_tacho_ff[2][2] = {{-1, -1}, {-1, -1}}; +inline double host_tacho_trim[2] = {-1, -1}; +inline unsigned host_tacho_ff_calls[2] = {0, 0}; class DCMotorTacho { @@ -21,14 +25,19 @@ class DCMotorTacho requested = rpm; host_tacho_requested[servo->index] = rpm; } - void setPIDTunings(double kp, double ki, double kd) + void setPIDTunings(double kp, double ki) { host_tacho_tunings[servo->index][0] = kp; host_tacho_tunings[servo->index][1] = ki; - host_tacho_tunings[servo->index][2] = kd; host_tacho_tunings_calls[servo->index]++; } - void setMaxLead(long counts) { host_tacho_max_lead[servo->index] = counts; } + void setFeedforward(double pwmStatic, double kv) + { + host_tacho_ff[servo->index][0] = pwmStatic; + host_tacho_ff[servo->index][1] = kv; + host_tacho_ff_calls[servo->index]++; + } + void setTrimLimit(double pwm) { host_tacho_trim[servo->index] = pwm; } double getMeasuredSpeedRPM() const { return 0; } void run() { servo->run(); } void stop() { servo->stop(); } diff --git a/firmware/test/host/test_dispatch.cpp b/firmware/test/host/test_dispatch.cpp index bac7986..4c6a891 100644 --- a/firmware/test/host/test_dispatch.cpp +++ b/firmware/test/host/test_dispatch.cpp @@ -10,7 +10,7 @@ void setup(); static const uint8_t COMMANDS[] = { DCMT_OP_SET_OPEN_LOOP, DCMT_OP_SET_BRAKE, DCMT_OP_SET_MODE, DCMT_OP_SET_SETPOINT, - DCMT_OP_SET_PID, BREAD_OP_SET_WATCHDOG, BREAD_OP_CLEAR_WATCHDOG_TRIP, BREAD_OP_CLEAR_FAULTS, + DCMT_OP_SET_PID, DCMT_OP_SET_SPEED_FF, BREAD_OP_SET_WATCHDOG, BREAD_OP_CLEAR_WATCHDOG_TRIP, BREAD_OP_CLEAR_FAULTS, }; static const uint8_t QUERIES[] = {DCMT_OP_GET_STATE, BREAD_OP_GET_WATCHDOG, BREAD_OP_GET_CAPS, BREAD_OP_GET_FAULTS, #if DCMT_HAS_ANALOG_INPUTS diff --git a/firmware/test/host/test_max_lead.cpp b/firmware/test/host/test_max_lead.cpp deleted file mode 100644 index c460407..0000000 --- a/firmware/test/host/test_max_lead.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// setup() caps both speed loops' lead at DCMT_TACHO_MAX_LEAD where the speed -// loop is built (#63). Without the cap a held shaft banks travel and lunges -// on release. -#include "host.h" -#include "config.h" -#include "config_hardware.h" -#include - -void setup(); - -int main() -{ - setup(); - -#if DCMT_ENABLE_SPEED_LOOP - host_check("motor 1 lead capped", host_tacho_max_lead[0] == DCMT_TACHO_MAX_LEAD); - host_check("motor 2 lead capped", host_tacho_max_lead[1] == DCMT_TACHO_MAX_LEAD); -#else - host_check("no speed loop, no cap set", host_tacho_max_lead[0] == 0 && host_tacho_max_lead[1] == 0); -#endif - return host_finish(); -} diff --git a/firmware/test/host/test_speed_baseline_90.cpp b/firmware/test/host/test_speed_baseline_90.cpp index b73cc6e..995719d 100644 --- a/firmware/test/host/test_speed_baseline_90.cpp +++ b/firmware/test/host/test_speed_baseline_90.cpp @@ -1,17 +1,15 @@ -// #90 regression: closed-loop speed at 60 RPM with the firmware's default -// speed-PID gains, the real DCMotorTacho, DCMotorServo and PID_v1, and the -// modelled motor. +// #90 regression: closed-loop speed at 60 RPM with the speed loop configured +// as sim_boot() sends it (Kp 0.6, Ki 3.0, the model motor's feedforward and +// trim 20), the real DCMotorTacho, DCMotorServo and PID_v1, and the modelled +// motor. // -// DCMotorTacho adds the speed PID's output, in counts, to the position target -// once per DCMT_TACHO_INTERVAL_MS, so the loop is P-only until the integral -// builds: speed = 15.04 * Kp * (target - speed) at 798 counts/rev and 5 ms, -// which settles at 69 % of the target with Kp 0.15. PID_v1 scales Ki by the -// sample time; with the old default Ki 0.01 the rest closed with a time -// constant of about 20 s (#90). Ki 0.2 reaches the target within 5 % in -// about 1.3 s. +// Under DCMotorServo 1.x the speed PID moved the position target and settled +// at 69 % of the target for seconds before the integral closed the rest +// (#90). DCMotorServo 2.x drives PWM as feedforward plus a PI trim, so the +// feedforward carries the speed and the trim closes the rest. // // The motor is the test's first-order model (motor_sim.h), fitted to one -// bench motor; the firmware carries none of it, and the default gains are a +// bench motor; the firmware carries none of it, and the gains are a // starting point for that motor, not for every motor. #include "host.h" #include "motor_sim.h" diff --git a/firmware/test/host/test_speed_entry_62.cpp b/firmware/test/host/test_speed_entry_62.cpp index 8687fcd..24abaa5 100644 --- a/firmware/test/host/test_speed_entry_62.cpp +++ b/firmware/test/host/test_speed_entry_62.cpp @@ -1,16 +1,18 @@ // #62: entering closed-loop speed after open-loop travel starts from where -// the shaft is, not from the servo target and speed baseline the open-loop -// travel left behind. DCMotorServo 1.2.0 does that (DCMotorServo#13): -// stop(), which apply_mode_transition() calls, syncs the target to the -// encoder, and setSpeedRPM() from 0 restarts the speed sample there. Without -// either, the first passes drive toward the stale target: at the PWM limit, -// or backwards. Each case boots cold, so it runs as its own process. +// the shaft is, not from the speed baseline the open-loop travel left +// behind. In DCMotorServo 2.x, setSpeedRPM() from 0 restarts the speed +// measurement and the integral there; with the shaft still coasting +// backwards, the first samples read it, and the loop must still never drive +// backwards for a forward target (#62). Each case boots cold, so it runs as +// its own process. #include "host.h" #include "motor_sim.h" #include "globals.h" #include #include #include +#include +#include // Motor 1's applied PWM over (from, to]. static void pwm_range(unsigned long from, unsigned long to, int &lo, int &hi) @@ -69,15 +71,80 @@ static void forward_stop_then_speed() host_check("#62 forward: the shaft never moves back from where it entered", low >= pe); } +// The reverse case over the entry timing: open loop at full reverse for each +// of 550 to 750 ms in 2 ms steps, then speed mode at +60 RPM. Where the entry +// falls against the speed interval and the coasting shaft decides what the +// first speed samples read, so one timing passing is no evidence for the +// next. Each timing boots cold, so each runs in a forked child, which sends +// back its lowest PWM over the first 1 s and whether its commands applied. +static void reverse_sweep() +{ + const unsigned long first = 550, last = 750, step = 2; + int runs = 0, negative = 0, broken = 0, lowest = 255; + for (unsigned long ms = first; ms <= last; ms += step) + { + int fd[2]; + if (pipe(fd) != 0) + { + host_check("pipe() for the sweep", false); + return; + } + fflush(stdout); + const pid_t pid = fork(); + if (pid == 0) + { + close(fd[0]); + if (!freopen("/dev/null", "w", stdout)) + _exit(3); + sim_boot(); + const bool ok = sim_open_loop(-255, 0); + sim_run(ms); + const unsigned long te = host_now; + sim_speed_run(60, 1000); + int lo, hi; + pwm_range(te, te + 1000, lo, hi); + const int msg[2] = {lo, (ok && host_finish() == 0) ? 1 : 0}; + _exit(write(fd[1], msg, sizeof msg) == (ssize_t)sizeof msg ? 0 : 3); + } + close(fd[1]); + int msg[2] = {0, 0}; + const bool got = pid > 0 && read(fd[0], msg, sizeof msg) == (ssize_t)sizeof msg; + close(fd[0]); + int status = 0; + if (pid > 0) + waitpid(pid, &status, 0); + runs++; + if (!got || !WIFEXITED(status) || WEXITSTATUS(status) != 0 || msg[1] != 1) + { + printf(" %lu ms: the case didn't run to the end\n", ms); + broken++; + continue; + } + if (msg[0] < 0) + { + printf(" %lu ms: lowest PWM %d in the first 1 s\n", ms, msg[0]); + negative++; + } + lowest = msg[0] < lowest ? msg[0] : lowest; + } + printf(" %d entry timings, %lu to %lu ms: %d with negative PWM; lowest PWM %d\n", runs, first, last, negative, + lowest); + host_check("#62 sweep: every timing ran, its commands applied", broken == 0 && runs == 101); + host_check("#62 sweep: entering speed at +60 RPM after 550-750 ms of full reverse never drives backwards", + negative == 0); +} + int main(int argc, char **argv) { if (argc > 1 && strcmp(argv[1], "reverse") == 0) reverse_then_speed(); else if (argc > 1 && strcmp(argv[1], "forward") == 0) forward_stop_then_speed(); + else if (argc > 1 && strcmp(argv[1], "sweep") == 0) + reverse_sweep(); else { - printf("usage: %s reverse|forward\n", argv[0]); + printf("usage: %s reverse|forward|sweep\n", argv[0]); return 2; } return host_finish(); diff --git a/firmware/test/host/test_speed_ff.cpp b/firmware/test/host/test_speed_ff.cpp new file mode 100644 index 0000000..1f6acd5 --- /dev/null +++ b/firmware/test/host/test_speed_ff.cpp @@ -0,0 +1,200 @@ +// The speed loop's gains and feedforward (module major 3, DCMotorServo 2.x): +// GET_VERSION reports major 3; SET_PID's speed gains reach each tacho as +// setPIDTunings(kp, ki); SET_SPEED_FF and serial SPEEDFF= store each motor's +// static, RPM per PWM step and trim, and the loop applies them as +// setFeedforward(static, kv) with kv in counts/s per PWM step at that +// motor's CPR, and setTrimLimit(trim). Until one is sent: no feedforward and +// trim 255, a plain PI. A wrong length is refused MALFORMED and stores +// nothing; a build without the speed loop refuses it NOT_BUILT, has no +// SPEEDFF=, and doesn't advertise DCMT_CAP_SPEED_FEEDFORWARD. +#include "host.h" +#include "globals.h" +#include +#include +#include +#include +#include +#include + +void setup(); +void loop(); + +static bool refused(uint8_t reason) +{ + const crumbs_ack_t a = host_read_ack(); + return a.status == CRUMBS_ACK_REFUSED && a.reason == reason; +} + +static void put16(uint8_t *p, uint16_t v) +{ + p[0] = (uint8_t)(v & 0xFF); + p[1] = (uint8_t)(v >> 8); +} + +static void set_speed_ff(uint8_t s1, uint16_t r1, uint8_t t1, uint8_t s2, uint16_t r2, uint8_t t2, uint8_t len = 8) +{ + uint8_t p[9] = {s1, 0, 0, t1, s2, 0, 0, t2, 0}; + put16(p + 1, r1); + put16(p + 5, r2); + host_send(DCMT_TYPE_ID, DCMT_OP_SET_SPEED_FF, p, len); +} + +static void serial(const char *line) +{ + Serial.out.clear(); + Serial.in += line; + Serial.in += "\n"; + loop(); +} + +static void passes(int n) +{ + for (int i = 0; i < n; i++) + loop(); +} + +#if DCMT_ENABLE_SPEED_LOOP +static bool stored(const SpeedFeedforward &f, uint8_t s, uint16_t r, uint8_t t) +{ + return f.pwmStatic == s && f.rpmPerPwmX1000 == r && f.trim == t; +} + +// What motor m's tacho was last given: static, kv (counts/s per PWM step) +// and trim. +static bool tacho_ff_is(int m, double s, double kv, double trim) +{ + return fabs(host_tacho_ff[m][0] - s) < 1e-6 && fabs(host_tacho_ff[m][1] - kv) < 1e-6 && + fabs(host_tacho_trim[m] - trim) < 1e-6; +} + +// kv for an RPM per PWM step x1000 at a CPR. +static double kv_of(uint16_t rpp_x1000, double cpr) { return rpp_x1000 / 1000.0 * cpr / 60.0; } + +static bool tunings_are(int m, double kp, double ki) +{ + return fabs(host_tacho_tunings[m][0] - kp) < 1e-4 && fabs(host_tacho_tunings[m][1] - ki) < 1e-4; +} +#endif + +int main() +{ + setup(); + loop(); + + crumbs_message_t reply; + const bool got_version = host_query(0x00, &reply) && reply.data_len >= 5; + printf(" GET_VERSION: %u.%u.%u\n", reply.data[2], reply.data[3], reply.data[4]); + host_check("GET_VERSION reports module major 3", got_version && reply.data[2] == 3 && DCMT_MODULE_VER_MAJOR == 3); + + bread_caps_result_t caps = {}; + const bool got_caps = + host_query(BREAD_OP_GET_CAPS, &reply) && bread_caps_parse_payload(reply.data, reply.data_len, &caps) == 0; + host_check("GET_CAPS answers", got_caps); + + // A wrong length is refused MALFORMED on every build, before NOT_BUILT. + set_speed_ff(10, 1719, 20, 10, 1719, 20, 7); + host_check("SET_SPEED_FF of 7 bytes is refused MALFORMED", refused(BREAD_REFUSE_MALFORMED)); + set_speed_ff(10, 1719, 20, 10, 1719, 20, 9); + host_check("SET_SPEED_FF of 9 bytes is refused MALFORMED", refused(BREAD_REFUSE_MALFORMED)); + +#if DCMT_ENABLE_SPEED_LOOP + host_check("GET_CAPS advertises DCMT_CAP_SPEED_FEEDFORWARD (bit 13) with DCMT_CAP_CLOSED_LOOP_SPEED", + (caps.flags & DCMT_CAP_SPEED_FEEDFORWARD) != 0 && (caps.flags & DCMT_CAP_CLOSED_LOOP_SPEED) != 0 && + DCMT_CAP_SPEED_FEEDFORWARD == (1u << 13)); + + // Defaults: the library's PI gains, no feedforward, no trim bound. + host_check("setup() gives both tachos the default speed gains, Kp 0.6 Ki 3.0", + tunings_are(0, 0.6, 3.0) && tunings_are(1, 0.6, 3.0)); + host_check("setup() gives both tachos no feedforward and trim 255 (a plain PI)", + tacho_ff_is(0, 0, 0, 255) && tacho_ff_is(1, 0, 0, 255)); + host_check("the malformed frames stored nothing", + stored(slice.speedFf1, 0, 0, 255) && stored(slice.speedFf2, 0, 0, 255)); + + // SET_PID speed: kp, ki reach the tachos; kd is stored and unused. + uint8_t pid[13] = {DCMT_PID_LOOP_SPEED}; + put16(pid + 1, 500); + put16(pid + 3, 2500); + put16(pid + 5, 20); + put16(pid + 7, 400); + put16(pid + 9, 2000); + put16(pid + 11, 30); + host_send(DCMT_TYPE_ID, DCMT_OP_SET_PID, pid, 13); + host_check("SET_PID speed is applied", host_read_ack().status == CRUMBS_ACK_APPLIED); + + // Motor 1 is the motor model's (static 10, 24.3 counts/s per PWM at + // CPR 848: 1.719 RPM per PWM), trim 20; motor 2 differs in every field. + set_speed_ff(10, 1719, 20, 12, 2000, 30); + host_check("SET_SPEED_FF is applied", host_read_ack().status == CRUMBS_ACK_APPLIED); + host_check("SET_SPEED_FF stores each motor's static, RPM per PWM and trim", + stored(slice.speedFf1, 10, 1719, 20) && stored(slice.speedFf2, 12, 2000, 30)); + + const uint8_t sp[5] = {DCMT_PID_LOOP_SPEED, 60, 0, 60, 0}; + host_send(DCMT_TYPE_ID, DCMT_OP_SET_SETPOINT, sp, 5); + const uint8_t mode = DCMT_MODE_CLOSED_SPEED; + host_send(DCMT_TYPE_ID, DCMT_OP_SET_MODE, &mode, 1); + host_check("speed mode is applied", host_read_ack().status == CRUMBS_ACK_APPLIED); + passes(5); + + host_check("both tachos run the runtime speed gains (kp, ki)", + tunings_are(0, 0.5, 2.5) && tunings_are(1, 0.4, 2.0)); + printf(" motor 1: static %.0f kv %.4f trim %.0f; motor 2: static %.0f kv %.4f trim %.0f\n", host_tacho_ff[0][0], + host_tacho_ff[0][1], host_tacho_trim[0], host_tacho_ff[1][0], host_tacho_ff[1][1], host_tacho_trim[1]); + host_check("motor 1's tacho gets static 10, kv 1.719 * MOTOR1_CPR / 60 counts/s per PWM, trim 20", + tacho_ff_is(0, 10, kv_of(1719, MOTOR1_CPR), 20)); + host_check("at CPR 848 that kv is the motor model's 24.3 counts/s per PWM, within 0.01", + MOTOR1_CPR != 848 || fabs(host_tacho_ff[0][1] - 24.3) < 0.01); + host_check("motor 2's tacho gets static 12, kv 2.0 * MOTOR2_CPR / 60, trim 30", + tacho_ff_is(1, 12, kv_of(2000, MOTOR2_CPR), 30)); + + const unsigned calls = host_tacho_ff_calls[0]; + passes(20); + host_check("the feedforward is set once, not on every pass", host_tacho_ff_calls[0] == calls); + + // rpm_per_pwm 0 is no speed term: kv 0. Trim 255 is unbounded. + set_speed_ff(0, 0, 255, 5, 0, 255); + host_check("SET_SPEED_FF with no speed term is applied", host_read_ack().status == CRUMBS_ACK_APPLIED); + passes(5); + host_check("rpm_per_pwm 0 reaches the tacho as kv 0, trim 255 as 255", + tacho_ff_is(0, 0, 0, 255) && tacho_ff_is(1, 5, 0, 255)); + + // A configuration write, as SET_PID: taken while the e-stop is held. + host_estop_level = LOW; + loop(); + set_speed_ff(8, 1500, 25, 8, 1500, 25); + host_check("SET_SPEED_FF while the e-stop is held is applied, as SET_PID is", + host_read_ack().status == CRUMBS_ACK_APPLIED && stored(slice.speedFf1, 8, 1500, 25)); + + // A malformed frame leaves what was stored. + set_speed_ff(1, 1, 1, 1, 1, 1, 7); + host_check("a malformed SET_SPEED_FF after a good one is refused and changes nothing", + refused(BREAD_REFUSE_MALFORMED) && stored(slice.speedFf1, 8, 1500, 25) && + stored(slice.speedFf2, 8, 1500, 25)); + + // Serial SPEEDFF=: the same six fields, rpp in RPM per PWM step. + serial("SPEEDFF=10,1.719,20,12,2,30"); + host_check("SPEEDFF=10,1.719,20,12,2,30 is applied", + Serial.out.find("updated") != std::string::npos && stored(slice.speedFf1, 10, 1719, 20) && + stored(slice.speedFf2, 12, 2000, 30)); + const char *bad[] = {"SPEEDFF=10,1.719,20", "SPEEDFF=10,1.719,20,12,2,30,1", "SPEEDFF=256,1,20,0,0,0", + "SPEEDFF=10,70,20,0,0,0", "SPEEDFF=10,-1,20,0,0,0", "SPEEDFF=10,1,256,0,0,0", + "SPEEDFF=10,nan,20,0,0,0", "SPEEDFF=1.5,1,20,0,0,0", "SPEEDFF=10,1,20,0,0,x"}; + for (const char *line : bad) + { + serial(line); + char what[96]; + snprintf(what, sizeof what, "%s is refused with no change", line); + host_check(what, Serial.out.find("format:") != std::string::npos && stored(slice.speedFf1, 10, 1719, 20) && + stored(slice.speedFf2, 12, 2000, 30)); + } +#else + host_check("GET_CAPS doesn't advertise DCMT_CAP_SPEED_FEEDFORWARD without the speed loop", + (caps.flags & DCMT_CAP_SPEED_FEEDFORWARD) == 0 && (caps.flags & DCMT_CAP_CLOSED_LOOP_SPEED) == 0); + set_speed_ff(10, 1719, 20, 10, 1719, 20); + host_check("SET_SPEED_FF is refused NOT_BUILT without the speed loop", refused(BREAD_REFUSE_NOT_BUILT)); + serial("SPEEDFF=10,1.719,20,10,1.719,20"); + host_check("serial SPEEDFF= isn't a command without the speed loop", + Serial.out.find("Invalid command.") != std::string::npos); + passes(2); +#endif + return host_finish(); +} diff --git a/firmware/test/host/test_speed_freeze.cpp b/firmware/test/host/test_speed_freeze.cpp deleted file mode 100644 index e98287e..0000000 --- a/firmware/test/host/test_speed_freeze.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// The speed loop freezes its integral while the lead cap clamps (#89): in -// closed-loop speed, a motor whose position target sits DCMT_TACHO_MAX_LEAD -// or more counts from its encoder runs the speed PID with Ki 0, and the -// other motor, and the same motor once the lead drops, run the gains in -// force. Those are the runtime gains from SET_PID, not the compile-time -// defaults, clamped or not. -#include "host.h" -#include "globals.h" -#include -#include -#include -#include - -void setup(); -void loop(); - -#if DCMT_ENABLE_SPEED_LOOP -static void put16(uint8_t *p, uint16_t v) -{ - p[0] = (uint8_t)(v & 0xFF); - p[1] = (uint8_t)(v >> 8); -} - -// SET_PID on the speed loop, gains x1000. -static bool set_speed_pid(uint16_t kp1, uint16_t ki1, uint16_t kd1, uint16_t kp2, uint16_t ki2, uint16_t kd2) -{ - uint8_t p[13] = {DCMT_PID_LOOP_SPEED}; - put16(p + 1, kp1); - put16(p + 3, ki1); - put16(p + 5, kd1); - put16(p + 7, kp2); - put16(p + 9, ki2); - put16(p + 11, kd2); - host_send(DCMT_TYPE_ID, DCMT_OP_SET_PID, p, 13); - return host_read_ack().status == CRUMBS_ACK_APPLIED; -} - -static bool tunings_are(int m, double kp, double ki, double kd) -{ - const double *t = host_tacho_tunings[m]; - return fabs(t[0] - kp) < 1e-4 && fabs(t[1] - ki) < 1e-4 && fabs(t[2] - kd) < 1e-4; -} - -static void passes(int n) -{ - for (int i = 0; i < n; i++) - loop(); -} -#endif - -int main() -{ - setup(); - loop(); - -#if DCMT_ENABLE_SPEED_LOOP - host_check("SET_PID speed is applied", set_speed_pid(500, 300, 20, 400, 250, 30)); - const uint8_t sp[5] = {DCMT_PID_LOOP_SPEED, 60, 0, 60, 0}; - host_send(DCMT_TYPE_ID, DCMT_OP_SET_SETPOINT, sp, 5); - const uint8_t mode = DCMT_MODE_CLOSED_SPEED; - host_send(DCMT_TYPE_ID, DCMT_OP_SET_MODE, &mode, 1); - host_check("speed mode is applied", host_read_ack().status == CRUMBS_ACK_APPLIED); - passes(5); - - host_check("unclamped, both motors run the runtime gains", - tunings_are(0, 0.5, 0.3, 0.02) && tunings_are(1, 0.4, 0.25, 0.03)); - - // Encoder far behind the target: lead >= DCMT_TACHO_MAX_LEAD. - host_servo_actual[0] = -100000; - passes(5); - printf(" motor 1 clamped: kp %.3f ki %.3f kd %.3f\n", host_tacho_tunings[0][0], host_tacho_tunings[0][1], - host_tacho_tunings[0][2]); - host_check("#89: a positive lead at the cap runs motor 1 with Ki 0 and the runtime Kp, Kd", - tunings_are(0, 0.5, 0, 0.02)); - host_check("#89: and motor 2, unclamped, keeps its runtime gains", tunings_are(1, 0.4, 0.25, 0.03)); - - host_servo_actual[0] = 100000; - passes(5); - host_check("#89: a negative lead at the cap freezes it too", tunings_are(0, 0.5, 0, 0.02)); - - host_check("SET_PID speed while clamped is applied", set_speed_pid(700, 400, 50, 400, 250, 30)); - passes(5); - host_check("#89: a SET_PID while clamped takes effect, still with Ki 0", tunings_are(0, 0.7, 0, 0.05)); - - const unsigned calls = host_tacho_tunings_calls[0]; - passes(20); - host_check("the frozen gains are set once, not on every pass", host_tacho_tunings_calls[0] == calls); - - host_servo_actual[0] = 0; - passes(5); - host_check("#89: once the lead drops, motor 1 runs the runtime gains with their Ki", - tunings_are(0, 0.7, 0.4, 0.05)); -#else - printf(" skip: the speed loop isn't built in this env\n"); -#endif - return host_finish(); -} diff --git a/firmware/test/host/test_stall_release_89.cpp b/firmware/test/host/test_stall_release_89.cpp index 6431b51..27575d9 100644 --- a/firmware/test/host/test_stall_release_89.cpp +++ b/firmware/test/host/test_stall_release_89.cpp @@ -1,9 +1,8 @@ // #89 regression: at 60 RPM, motor 1's shaft is held and then released. -// While it is held, the lead cap (DCMT_TACHO_MAX_LEAD) stops the position -// target 300 counts ahead. Before the fix, the speed PID's integral kept -// winding up against the cap, and the shaft lunged on release. The firmware -// now runs the speed PID with Ki 0 while the cap clamps, so the integral is -// frozen and the shaft returns to about the target. +// Under DCMotorServo 1.x the speed PID's integral wound up while the shaft +// was held and the shaft lunged on release. DCMotorServo 2.x bounds the PI +// trim (SET_SPEED_FF trim 20, sim_boot()) and doesn't integrate while the +// trim is at that bound, so the shaft returns to about the target. // // A hold of DCMT_STALL_TIMEOUT_MS or more latches a stall fault (#64), so the // motor stays braked when the shaft is let go: the 4 s hold of #89's report diff --git a/slice.yaml b/slice.yaml index fd2ba37..b593abf 100644 --- a/slice.yaml +++ b/slice.yaml @@ -117,9 +117,9 @@ firmware: - "gen2_nanoevery_analog1" lib_deps: - "cameronbrooks11/CRUMBS @ ^0.17.0" - - "cameronbrooks11/bread-crumbs-contracts @ ^0.12.0" + - "cameronbrooks11/bread-crumbs-contracts @ ^0.13.0" - "cameronbrooks11/LMD18200 @ ^0.1.5" - - "cameronbrooks11/DCMotorServo @ ^1.2.1" + - "cameronbrooks11/DCMotorServo @ ^2.0.0" - "paulstoffregen/Encoder @ ^1.4.4" - "cameronbrooks11/bread-slice-core @ ^0.2.0"