Skip to content

Commit 5c73b2e

Browse files
author
Richard Unger
committed
refactoring the SimpleFOCRegisters
1 parent 948a8f5 commit 5c73b2e

File tree

7 files changed

+237
-59
lines changed

7 files changed

+237
-59
lines changed

‎src/comms/SimpleFOCRegisters.cpp‎

Lines changed: 166 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -165,17 +165,32 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
165165
case SimpleFOCRegister::REG_VEL_PID_D:
166166
comms << motor->PID_velocity.D;
167167
break;
168+
case SimpleFOCRegister::REG_VEL_PID_LIM:
169+
comms << motor->PID_velocity.limit;
170+
break;
171+
case SimpleFOCRegister::REG_VEL_PID_RAMP:
172+
comms << motor->PID_velocity.output_ramp;
173+
break;
168174
case SimpleFOCRegister::REG_VEL_LPF_T:
169175
comms << motor->LPF_velocity.Tf;
170176
break;
171177
case SimpleFOCRegister::REG_ANG_PID_P:
172178
comms << motor->P_angle.P;
173179
break;
174-
case SimpleFOCRegister::REG_VEL_LIMIT:
175-
comms << motor->velocity_limit;
180+
case SimpleFOCRegister::REG_ANG_PID_I:
181+
comms << motor->P_angle.I;
176182
break;
177-
case SimpleFOCRegister::REG_VEL_MAX_RAMP:
178-
comms << motor->PID_velocity.output_ramp;
183+
case SimpleFOCRegister::REG_ANG_PID_D:
184+
comms << motor->P_angle.D;
185+
break;
186+
case SimpleFOCRegister::REG_ANG_PID_LIM:
187+
comms << motor->P_angle.limit;
188+
break;
189+
case SimpleFOCRegister::REG_ANG_PID_RAMP:
190+
comms << motor->P_angle.output_ramp;
191+
break;
192+
case SimpleFOCRegister::REG_ANG_LPF_T:
193+
comms << motor->LPF_angle.Tf;
179194
break;
180195

181196
case SimpleFOCRegister::REG_CURQ_PID_P:
@@ -187,6 +202,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
187202
case SimpleFOCRegister::REG_CURQ_PID_D:
188203
comms << motor->PID_current_q.D;
189204
break;
205+
case SimpleFOCRegister::REG_CURQ_PID_LIM:
206+
comms << motor->PID_current_q.limit;
207+
break;
208+
case SimpleFOCRegister::REG_CURQ_PID_RAMP:
209+
comms << motor->PID_current_q.output_ramp;
210+
break;
190211
case SimpleFOCRegister::REG_CURQ_LPF_T:
191212
comms << motor->LPF_current_q.Tf;
192213
break;
@@ -199,6 +220,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
199220
case SimpleFOCRegister::REG_CURD_PID_D:
200221
comms << motor->PID_current_d.D;
201222
break;
223+
case SimpleFOCRegister::REG_CURD_PID_LIM:
224+
comms << motor->PID_current_d.limit;
225+
break;
226+
case SimpleFOCRegister::REG_CURD_PID_RAMP:
227+
comms << motor->PID_current_d.output_ramp;
228+
break;
202229
case SimpleFOCRegister::REG_CURD_LPF_T:
203230
comms << motor->LPF_current_d.Tf;
204231
break;
@@ -209,6 +236,9 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
209236
case SimpleFOCRegister::REG_CURRENT_LIMIT:
210237
comms << motor->current_limit;
211238
break;
239+
case SimpleFOCRegister::REG_VELOCITY_LIMIT:
240+
comms << motor->velocity_limit;
241+
break;
212242
case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE:
213243
comms << (uint8_t)motor->motion_downsample;
214244
break;
@@ -241,13 +271,49 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
241271
comms << (uint8_t)motor->pole_pairs;
242272
break;
243273

274+
// case SimpleFOCRegister::REG_CURA_GAIN:
275+
// if (motor->current_sense)
276+
// comms << motor->current_sense->gain_a;
277+
// else
278+
// comms << 0.0f;
279+
// break;
280+
// case SimpleFOCRegister::REG_CURB_GAIN:
281+
// if (motor->current_sense)
282+
// comms << motor->current_sense->gain_b;
283+
// else
284+
// comms << 0.0f;
285+
// break;
286+
// case SimpleFOCRegister::REG_CURC_GAIN:
287+
// if (motor->current_sense)
288+
// comms << motor->current_sense->gain_c;
289+
// else
290+
// comms << 0.0f;
291+
// break;
292+
// case SimpleFOCRegister::REG_CURA_OFFSET:
293+
// if (motor->current_sense)
294+
// comms << motor->current_sense->offset_a;
295+
// else
296+
// comms << 0.0f;
297+
// break;
298+
// case SimpleFOCRegister::REG_CURB_OFFSET:
299+
// if (motor->current_sense)
300+
// comms << motor->current_sense->offset_b;
301+
// else
302+
// comms << 0.0f;
303+
// break;
304+
// case SimpleFOCRegister::REG_CURC_OFFSET:
305+
// if (motor->current_sense)
306+
// comms << motor->current_sense->offset_c;
307+
// else
308+
// comms << 0.0f;
309+
// break;
310+
244311
case SimpleFOCRegister::REG_SYS_TIME:
245312
// TODO how big is millis()? Same on all platforms?
246313
comms << (uint32_t)(int)millis();
247314
break;
248315
// unknown register or write only register (no read) or can't handle in superclass
249316
case SimpleFOCRegister::REG_NUM_MOTORS:
250-
case SimpleFOCRegister::REG_REPORT:
251317
case SimpleFOCRegister::REG_MOTOR_ADDRESS:
252318
case SimpleFOCRegister::REG_ENABLE_ALL:
253319
default:
@@ -338,17 +404,32 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
338404
case SimpleFOCRegister::REG_VEL_PID_D:
339405
comms >> (motor->PID_velocity.D);
340406
return true;
407+
case SimpleFOCRegister::REG_VEL_PID_LIM:
408+
comms >> (motor->PID_velocity.limit);
409+
return true;
410+
case SimpleFOCRegister::REG_VEL_PID_RAMP:
411+
comms >> (motor->PID_velocity.output_ramp);
412+
return true;
341413
case SimpleFOCRegister::REG_VEL_LPF_T:
342414
comms >> (motor->LPF_velocity.Tf);
343415
return true;
344416
case SimpleFOCRegister::REG_ANG_PID_P:
345417
comms >> (motor->P_angle.P);
346418
return true;
347-
case SimpleFOCRegister::REG_VEL_LIMIT:
348-
comms >> (motor->velocity_limit);
419+
case SimpleFOCRegister::REG_ANG_PID_I:
420+
comms >> (motor->P_angle.I);
349421
return true;
350-
case SimpleFOCRegister::REG_VEL_MAX_RAMP:
351-
comms >> (motor->PID_velocity.output_ramp);
422+
case SimpleFOCRegister::REG_ANG_PID_D:
423+
comms >> (motor->P_angle.D);
424+
return true;
425+
case SimpleFOCRegister::REG_ANG_PID_LIM:
426+
comms >> (motor->P_angle.limit);
427+
return true;
428+
case SimpleFOCRegister::REG_ANG_PID_RAMP:
429+
comms >> (motor->P_angle.output_ramp);
430+
return true;
431+
case SimpleFOCRegister::REG_ANG_LPF_T:
432+
comms >> (motor->LPF_angle.Tf);
352433
return true;
353434

354435
case SimpleFOCRegister::REG_CURQ_PID_P:
@@ -360,6 +441,12 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
360441
case SimpleFOCRegister::REG_CURQ_PID_D:
361442
comms >> (motor->PID_current_q.D);
362443
return true;
444+
case SimpleFOCRegister::REG_CURQ_PID_LIM:
445+
comms >> (motor->PID_current_q.limit);
446+
return true;
447+
case SimpleFOCRegister::REG_CURQ_PID_RAMP:
448+
comms >> (motor->PID_current_q.output_ramp);
449+
return true;
363450
case SimpleFOCRegister::REG_CURQ_LPF_T:
364451
comms >> (motor->LPF_current_q.Tf);
365452
return true;
@@ -372,15 +459,36 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
372459
case SimpleFOCRegister::REG_CURD_PID_D:
373460
comms >> (motor->PID_current_d.D);
374461
return true;
462+
case SimpleFOCRegister::REG_CURD_PID_LIM:
463+
comms >> (motor->PID_current_d.limit);
464+
return true;
465+
case SimpleFOCRegister::REG_CURD_PID_RAMP:
466+
comms >> (motor->PID_current_d.output_ramp);
467+
return true;
375468
case SimpleFOCRegister::REG_CURD_LPF_T:
376469
comms >> (motor->LPF_current_d.Tf);
377470
return true;
378471

379472
case SimpleFOCRegister::REG_VOLTAGE_LIMIT:
380473
comms >> (motor->voltage_limit);
474+
if (motor->phase_resistance==NOT_SET){
475+
motor->PID_velocity.limit = motor->voltage_limit;
476+
if (motor->controller==MotionControlType::angle_nocascade)
477+
motor->P_angle.limit = motor->voltage_limit;
478+
}
381479
return true;
382480
case SimpleFOCRegister::REG_CURRENT_LIMIT:
383481
comms >> (motor->current_limit);
482+
if (motor->phase_resistance!=NOT_SET) {
483+
motor->PID_velocity.limit = motor->current_limit;
484+
if (motor->controller==MotionControlType::angle_nocascade)
485+
motor->P_angle.limit = motor->current_limit;
486+
}
487+
return true;
488+
case SimpleFOCRegister::REG_VELOCITY_LIMIT:
489+
comms >> (motor->velocity_limit);
490+
if (motor->controller!=MotionControlType::angle_nocascade)
491+
motor->P_angle.limit = motor->velocity_limit;
384492
return true;
385493
case SimpleFOCRegister::REG_MOTION_DOWNSAMPLE:
386494
comms >> val8;
@@ -417,6 +525,38 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
417525
comms >> val8;
418526
motor->pole_pairs = val8;
419527
return true;
528+
529+
// case SimpleFOCRegister::REG_CURA_GAIN:
530+
// comms >> va;
531+
// if (motor->current_sense)
532+
// motor->current_sense->gain_a = va;
533+
// return true;
534+
// case SimpleFOCRegister::REG_CURB_GAIN:
535+
// comms >> vb;
536+
// if (motor->current_sense)
537+
// motor->current_sense->gain_b = vb;
538+
// return true;
539+
// case SimpleFOCRegister::REG_CURC_GAIN:
540+
// comms >> vc;
541+
// if (motor->current_sense)
542+
// motor->current_sense->gain_c = vc;
543+
// return true;
544+
// case SimpleFOCRegister::REG_CURA_OFFSET:
545+
// comms >> va;
546+
// if (motor->current_sense)
547+
// motor->current_sense->offset_a = va;
548+
// return true;
549+
// case SimpleFOCRegister::REG_CURB_OFFSET:
550+
// comms >> vb;
551+
// if (motor->current_sense)
552+
// motor->current_sense->offset_b = vb;
553+
// return true;
554+
// case SimpleFOCRegister::REG_CURC_OFFSET:
555+
// comms >> vc;
556+
// if (motor->current_sense)
557+
// motor->current_sense->offset_c = vc;
558+
// return true;
559+
420560
// unknown register or read-only register (no write) or can't handle in superclass
421561
case SimpleFOCRegister::REG_ITERATIONS_SEC:
422562
case SimpleFOCRegister::REG_STATUS:
@@ -439,7 +579,6 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
439579
case SimpleFOCRegister::REG_NUM_MOTORS:
440580
case SimpleFOCRegister::REG_MOTOR_ADDRESS:
441581
case SimpleFOCRegister::REG_ENABLE_ALL:
442-
case SimpleFOCRegister::REG_REPORT:
443582
default:
444583
return false;
445584
}
@@ -468,17 +607,26 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
468607
case SimpleFOCRegister::REG_VEL_PID_P:
469608
case SimpleFOCRegister::REG_VEL_PID_I:
470609
case SimpleFOCRegister::REG_VEL_PID_D:
610+
case SimpleFOCRegister::REG_VEL_PID_LIM:
611+
case SimpleFOCRegister::REG_VEL_PID_RAMP:
471612
case SimpleFOCRegister::REG_VEL_LPF_T:
472613
case SimpleFOCRegister::REG_ANG_PID_P:
473-
case SimpleFOCRegister::REG_VEL_LIMIT:
474-
case SimpleFOCRegister::REG_VEL_MAX_RAMP:
614+
case SimpleFOCRegister::REG_ANG_PID_I:
615+
case SimpleFOCRegister::REG_ANG_PID_D:
616+
case SimpleFOCRegister::REG_ANG_PID_LIM:
617+
case SimpleFOCRegister::REG_ANG_PID_RAMP:
618+
case SimpleFOCRegister::REG_ANG_LPF_T:
475619
case SimpleFOCRegister::REG_CURQ_PID_P:
476620
case SimpleFOCRegister::REG_CURQ_PID_I:
477621
case SimpleFOCRegister::REG_CURQ_PID_D:
622+
case SimpleFOCRegister::REG_CURQ_PID_LIM:
623+
case SimpleFOCRegister::REG_CURQ_PID_RAMP:
478624
case SimpleFOCRegister::REG_CURQ_LPF_T:
479625
case SimpleFOCRegister::REG_CURD_PID_P:
480626
case SimpleFOCRegister::REG_CURD_PID_I:
481627
case SimpleFOCRegister::REG_CURD_PID_D:
628+
case SimpleFOCRegister::REG_CURD_PID_LIM:
629+
case SimpleFOCRegister::REG_CURD_PID_RAMP:
482630
case SimpleFOCRegister::REG_CURD_LPF_T:
483631
case SimpleFOCRegister::REG_VOLTAGE_LIMIT:
484632
case SimpleFOCRegister::REG_CURRENT_LIMIT:
@@ -491,6 +639,12 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
491639
case SimpleFOCRegister::REG_INDUCTANCE:
492640
case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE:
493641
case SimpleFOCRegister::REG_ITERATIONS_SEC:
642+
case SimpleFOCRegister::REG_CURA_GAIN:
643+
case SimpleFOCRegister::REG_CURB_GAIN:
644+
case SimpleFOCRegister::REG_CURC_GAIN:
645+
case SimpleFOCRegister::REG_CURA_OFFSET:
646+
case SimpleFOCRegister::REG_CURB_OFFSET:
647+
case SimpleFOCRegister::REG_CURC_OFFSET:
494648
return 4;
495649
case SimpleFOCRegister::REG_SYS_TIME:
496650
return 4; // TODO how big is millis()? Same on all platforms?
@@ -521,7 +675,6 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
521675
else
522676
return 1;
523677
case SimpleFOCRegister::REG_DRIVER_ENABLE:
524-
case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature
525678
case SimpleFOCRegister::REG_ENABLE_ALL: // write-only
526679
default: // unknown register or write only register (no output) or can't handle in superclass
527680
return 0;

‎src/comms/SimpleFOCRegisters.h‎

Lines changed: 30 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,13 @@
1414

1515
typedef enum : uint8_t {
1616
REG_STATUS = 0x00, // RO - 1 byte (motor status)
17-
REG_MOTOR_ADDRESS = 0x01, // R/W - 1 byte
18-
REG_REPORT = 0x02, // R/W - Write: variable, Read: variable, up to 32 bytes
17+
REG_TARGET = 0x01, // R/W - float
1918
REG_ENABLE_ALL = 0x03, // WO - 1 byte
2019
REG_ENABLE = 0x04, // R/W - 1 byte
2120
REG_CONTROL_MODE = 0x05, // R/W - 1 byte
2221
REG_TORQUE_MODE = 0x06, // R/W - 1 byte
2322
REG_MODULATION_MODE = 0x07, // R/W - 1 byte
2423

25-
REG_TARGET = 0x08, // R/W - float
2624
REG_ANGLE = 0x09, // RO - float
2725
REG_POSITION = 0x10, // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes)
2826
REG_VELOCITY = 0x11, // RO - float
@@ -53,25 +51,36 @@ typedef enum : uint8_t {
5351
REG_VEL_PID_P = 0x30, // R/W - float
5452
REG_VEL_PID_I = 0x31, // R/W - float
5553
REG_VEL_PID_D = 0x32, // R/W - float
56-
REG_VEL_LPF_T = 0x33, // R/W - float
57-
REG_ANG_PID_P = 0x34, // R/W - float
58-
REG_VEL_LIMIT = 0x35, // R/W - float
59-
REG_VEL_MAX_RAMP = 0x36, // R/W - float
54+
REG_VEL_PID_LIM = 0x33, // R/W - float
55+
REG_VEL_PID_RAMP = 0x34, // R/W - float
56+
REG_VEL_LPF_T = 0x35, // R/W - float
57+
REG_ANG_PID_P = 0x36, // R/W - float
58+
REG_ANG_PID_I = 0x37, // R/W - float
59+
REG_ANG_PID_D = 0x38, // R/W - float
60+
REG_ANG_PID_LIM = 0x39, // R/W - float
61+
REG_ANG_PID_RAMP = 0x3A, // R/W - float
62+
REG_ANG_LPF_T = 0x3B, // R/W - float
6063

6164
REG_CURQ_PID_P = 0x40, // R/W - float
6265
REG_CURQ_PID_I = 0x41, // R/W - float
6366
REG_CURQ_PID_D = 0x42, // R/W - float
64-
REG_CURQ_LPF_T = 0x43, // R/W - float
65-
REG_CURD_PID_P = 0x44, // R/W - float
66-
REG_CURD_PID_I = 0x45, // R/W - float
67-
REG_CURD_PID_D = 0x46, // R/W - float
68-
REG_CURD_LPF_T = 0x47, // R/W - float
67+
REG_CURQ_PID_LIM = 0x43, // R/W - float
68+
REG_CURQ_PID_RAMP = 0x44, // R/W - float
69+
REG_CURQ_LPF_T = 0x45, // R/W - float
70+
REG_CURD_PID_P = 0x46, // R/W - float
71+
REG_CURD_PID_I = 0x47, // R/W - float
72+
REG_CURD_PID_D = 0x48, // R/W - float
73+
REG_CURD_PID_LIM = 0x49, // R/W - float
74+
REG_CURD_PID_RAMP = 0x4A, // R/W - float
75+
REG_CURD_LPF_T = 0x4B, // R/W - float
6976

7077
REG_VOLTAGE_LIMIT = 0x50, // R/W - float
7178
REG_CURRENT_LIMIT = 0x51, // R/W - float
72-
REG_MOTION_DOWNSAMPLE = 0x52, // R/W - uint32_t
79+
REG_VELOCITY_LIMIT = 0x52, // R/W - float
7380
REG_DRIVER_VOLTAGE_LIMIT = 0x53,// R/W - float
7481
REG_PWM_FREQUENCY = 0x54, // R/W - uint32_t
82+
REG_DRIVER_VOLTAGE_PSU = 0x55, // R/W - float
83+
REG_MOTION_DOWNSAMPLE = 0x5F, // R/W - uint32_t
7584

7685
REG_ZERO_ELECTRIC_ANGLE = 0x60, // RO - float
7786
REG_SENSOR_DIRECTION = 0x61, // RO - 1 byte
@@ -80,9 +89,17 @@ typedef enum : uint8_t {
8089
REG_PHASE_RESISTANCE = 0x64, // R/W - float
8190
REG_KV = 0x65, // R/W - float
8291
REG_INDUCTANCE = 0x66, // R/W - float
92+
REG_CURA_GAIN = 0x67, // R/W - float
93+
REG_CURB_GAIN = 0x68, // R/W - float
94+
REG_CURC_GAIN = 0x69, // R/W - float
95+
REG_CURA_OFFSET = 0x6A, // R/W - float
96+
REG_CURB_OFFSET = 0x6B, // R/W - float
97+
REG_CURC_OFFSET = 0x6C, // R/W - float
8398

8499
REG_NUM_MOTORS = 0x70, // RO - 1 byte
85100
REG_SYS_TIME = 0x71, // RO - uint32_t
101+
REG_MOTOR_ADDRESS = 0x7F, // R/W - float
102+
86103
} SimpleFOCRegister;
87104

88105

0 commit comments

Comments
 (0)