Skip to content

Commit 27f5215

Browse files
askuricaskuric
committed
commander refactor allow no serial
1 parent 39cf92e commit 27f5215

File tree

2 files changed

+215
-77
lines changed

2 files changed

+215
-77
lines changed

‎src/communication/Commander.cpp

Lines changed: 112 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -4,14 +4,20 @@
44
Commander::Commander(HardwareSerial& serial){
55
com_port = &serial;
66
}
7+
Commander::Commander(){
8+
// do nothing
9+
}
10+
711

812
void Commander::add(char id, CommandCallback onCommand){
913
call_list[call_count] = onCommand;
1014
call_ids[call_count] = id;
1115
call_count++;
1216
}
1317

18+
1419
void Commander::run(){
20+
if(!com_port) return;
1521
// a string to hold incoming data
1622
while (com_port->available()) {
1723
// get the new byte:
@@ -28,39 +34,45 @@ void Commander::run(){
2834
}
2935
}
3036

37+
void Commander::run(HardwareSerial &serial){
38+
// a string to hold incoming data
39+
while (serial.available()) {
40+
// get the new byte:
41+
received_chars[rec_cnt] = (char)serial.read();
42+
// end of user input
43+
if (received_chars[rec_cnt++] == '\n') {
44+
// execute the user command
45+
run(received_chars);
46+
47+
// reset the command buffer
48+
received_chars[0] = 0;
49+
rec_cnt=0;
50+
}
51+
}
52+
}
53+
3154
void Commander::run(char* user_input){
3255
// execute the user command
33-
char id = received_chars[0];
56+
char id = user_input[0];
3457
if(id == CMD_SCAN)
3558
for(int i=0; i < call_count; i++){
36-
com_port->print(call_ids[i]);
37-
com_port->print(":");
59+
print(call_ids[i], 0);
60+
print(":", 0);
3861
call_list[i](cmd_scan_msg);
3962
}
4063
else
4164
for(int i=0; i < call_count; i++){
4265
if(id == call_ids[i]){
43-
call_list[i](&received_chars[1]);
66+
call_list[i](&user_input[1]);
4467
break;
4568
}
4669
}
4770
}
4871

49-
void Commander::verbosePrint(const char* message){
50-
if(verbose) com_port->print(message);
51-
}
52-
void Commander::verbosePrint(const __FlashStringHelper *message){
53-
if(verbose) com_port->print(message);
54-
}
55-
void Commander::printNumber(const float number, const bool newline){
56-
if(newline) com_port->println(number,decimal_places);
57-
else com_port->print(number,decimal_places);
58-
}
59-
6072
void Commander::motor(FOCMotor* motor, char* user_command) {
6173
// if empty string
6274
if( user_command[0] == CMD_SCAN ){
63-
com_port->println(F("mot"));
75+
print(F("mot"),1);
6476
return;
6577
}
6678

@@ -107,7 +119,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
107119
// change velocity pid limit if in voltage mode and no phase resistance set
108120
if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
109121
}
110-
printNumber(motor->voltage_limit,1);
122+
print(motor->voltage_limit,1);
111123
break;
112124
case SCMD_LIM_CURR: // current limit
113125
verbosePrint(F("curr: "));
@@ -118,18 +130,18 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
118130
// if phase resistance specified or the current control is on set the current limit to the velocity PID
119131
if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
120132
}
121-
printNumber(motor->current_limit,1);
133+
print(motor->current_limit,1);
122134
break;
123135
case SCMD_LIM_VEL: // velocity limit
124136
verbosePrint(F("vel: "));
125137
if(!GET){
126138
motor->velocity_limit = value;
127139
motor->P_angle.limit = value;
128140
}
129-
printNumber(motor->velocity_limit,1);
141+
print(motor->velocity_limit,1);
130142
break;
131143
default:
132-
com_port->println(F("err"));
144+
printError();
133145
break;
134146
}
135147
break;
@@ -139,27 +151,27 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
139151
case SCMD_DOWNSAMPLE:
140152
verbosePrint(F("downsample: "));
141153
if(!GET) motor->motion_downsample = value;
142-
printNumber(motor->motion_downsample,1);
154+
print((int)motor->motion_downsample, 1);
143155
break;
144156
default:
145157
// change control type
146158
if(!GET && value >= 0 && (int)value < 5)// if set command
147159
motor->controller = (MotionControlType)value;
148160
switch(motor->controller){
149161
case MotionControlType::torque:
150-
com_port->println(F("torque"));
162+
print(F("torque"),1);
151163
break;
152164
case MotionControlType::velocity:
153-
com_port->println(F("vel"));
165+
print(F("vel"),1);
154166
break;
155167
case MotionControlType::angle:
156-
com_port->println(F("angle"));
168+
print(F("angle"),1);
157169
break;
158170
case MotionControlType::velocity_openloop:
159-
com_port->println(F("vel open"));
171+
print(F("vel open"),1);
160172
break;
161173
case MotionControlType::angle_openloop:
162-
com_port->println(F("angle open"));
174+
print(F("angle open"),1);
163175
break;
164176
}
165177
break;
@@ -172,21 +184,21 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
172184
motor->torque_controller = (TorqueControlType)value;
173185
switch(motor->torque_controller){
174186
case TorqueControlType::voltage:
175-
com_port->println(F("volt"));
187+
print(F("volt"),1);
176188
break;
177189
case TorqueControlType::current:
178-
com_port->println(F("curr"));
190+
print(F("curr"),1);
179191
break;
180192
case TorqueControlType::foc_current:
181-
com_port->println(F("foc"));
193+
print(F("foc"),1);
182194
break;
183195
}
184196
break;
185197
case CMD_STATUS:
186198
// enable/disable
187199
verbosePrint(F("Status: "));
188200
if(!GET) (bool)value ? motor->enable() : motor->disable();
189-
com_port->println(motor->enabled);
201+
print(motor->enabled,1);
190202
break;
191203
case CMD_RESIST:
192204
// enable/disable
@@ -198,8 +210,8 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
198210
motor->PID_velocity.limit= motor->current_limit;
199211
}
200212
}
201-
if(_isset(motor->phase_resistance)) printNumber(motor->phase_resistance,1);
202-
else com_port->println(0);
213+
if(_isset(motor->phase_resistance)) print(motor->phase_resistance,1);
214+
else print(0,0);
203215
break;
204216
case CMD_SENSOR:
205217
// Sensor zero offset
@@ -208,15 +220,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
208220
case SCMD_SENS_MECH_OFFSET: // zero offset
209221
verbosePrint(F("offset: "));
210222
if(!GET) motor->sensor_offset = value;
211-
printNumber(motor->sensor_offset,1);
223+
print(motor->sensor_offset,1);
212224
break;
213225
case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch
214226
verbosePrint(F("el. offset: "));
215227
if(!GET) motor->zero_electric_angle = value;
216-
printNumber(motor->zero_electric_angle,1);
228+
print(motor->zero_electric_angle,1);
217229
break;
218230
default:
219-
com_port->println(F("err"));
231+
printError();
220232
break;
221233
}
222234
break;
@@ -227,68 +239,68 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
227239
switch((uint8_t)value){
228240
case 0: // get target
229241
verbosePrint(F("target: "));
230-
printNumber(motor->target,1);
242+
print(motor->target,1);
231243
break;
232244
case 1: // get voltage q
233245
verbosePrint(F("Vq: "));
234-
printNumber(motor->voltage.q,1);
246+
print(motor->voltage.q,1);
235247
break;
236248
case 2: // get voltage d
237249
verbosePrint(F("Vd: "));
238-
printNumber(motor->voltage.q,1);
250+
print(motor->voltage.q,1);
239251
break;
240252
case 3: // get current q
241253
verbosePrint(F("Cq: "));
242-
printNumber(motor->voltage.q,1);
254+
print(motor->voltage.q,1);
243255
break;
244256
case 4: // get current d
245257
verbosePrint(F("Cd: "));
246-
printNumber(motor->voltage.q,1);
258+
print(motor->voltage.q,1);
247259
break;
248260
case 5: // get velocity
249261
verbosePrint(F("vel: "));
250-
printNumber(motor->shaft_velocity,1);
262+
print(motor->shaft_velocity,1);
251263
break;
252264
case 6: // get angle
253265
verbosePrint(F("Angle: "));
254-
printNumber(motor->shaft_angle,1);
266+
print(motor->shaft_angle,1);
255267
break;
256268
default:
257-
com_port->println(F("err"));
269+
printError();
258270
break;
259271
}
260272
break;
261273
case SCMD_DOWNSAMPLE:
262274
verbosePrint(F("downsample: "));
263275
if(!GET) motor->monitor_downsample = value;
264-
printNumber(motor->monitor_downsample,1);
276+
print((int)motor->monitor_downsample,1);
265277
break;
266278
case SCMD_CLEAR:
267279
for(int i=0; i<7; i++) motor->monitor_variables[i] = 0;
268-
com_port->println(F("clear"));
280+
print(F("clear"),1);
269281
break;
270282
case SCMD_SET:
271283
for(int i=0; i<7; i++){
272284
motor->monitor_variables[i] = user_command[value_index+i] - '0';
273-
com_port->print(motor->monitor_variables[i]);
285+
print(motor->monitor_variables[i],0);
274286
}
275-
com_port->println();
287+
print("",1);
276288
break;
277289
default:
278-
com_port->println(F("err"));
290+
printError();
279291
break;
280292
}
281293
break;
282294
default: // target change
283295
verbosePrint(F("Target: "));
284296
motor->target = atof(user_command);
285-
printNumber(motor->target,1);
297+
print(motor->target,1);
286298
}
287299
}
288300

289301
void Commander::pid(PIDController* pid, char* user_cmd){
290302
if( user_cmd[0] == CMD_SCAN ){
291-
com_port->println(F("pid"));
303+
print(F("pid"),1);
292304
return;
293305
}
294306
char cmd = user_cmd[0];
@@ -299,37 +311,37 @@ void Commander::pid(PIDController* pid, char* user_cmd){
299311
case SCMD_PID_P: // P gain change
300312
verbosePrint("P: ");
301313
if(!GET) pid->P = value;
302-
printNumber(pid->P,1);
314+
print(pid->P,1);
303315
break;
304316
case SCMD_PID_I: // I gain change
305317
verbosePrint("I: ");
306318
if(!GET) pid->I = value;
307-
printNumber(pid->I,1);
319+
print(pid->I,1);
308320
break;
309321
case SCMD_PID_D: // D gain change
310322
verbosePrint("D: ");
311323
if(!GET) pid->D = value;
312-
printNumber(pid->D,1);
324+
print(pid->D,1);
313325
break;
314326
case SCMD_PID_RAMP: // ramp change
315327
verbosePrint("ramp: ");
316328
if(!GET) pid->output_ramp = value;
317-
printNumber(pid->output_ramp,1);
329+
print(pid->output_ramp,1);
318330
break;
319331
case SCMD_PID_LIM: // limit change
320332
verbosePrint("limit: ");
321333
if(!GET) pid->limit = value;
322-
printNumber(pid->limit,1);
334+
print(pid->limit,1);
323335
break;
324336
default:
325-
com_port->println(F("err"));
337+
printError();
326338
break;
327339
}
328340
}
329341

330342
void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
331343
if( user_cmd[0] == CMD_SCAN ){
332-
com_port->println(F("lpf"));
344+
print(F("lpf"),1);
333345
return;
334346
}
335347
char cmd = user_cmd[0];
@@ -338,22 +350,60 @@ void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
338350

339351
switch (cmd){
340352
case SCMD_LPF_TF: // Tf value change
341-
verbosePrint("Tf: ");
353+
verbosePrint(F("Tf: "));
342354
if(!GET) lpf->Tf = value;
343-
printNumber(lpf->Tf,1);
355+
print(lpf->Tf,1);
344356
break;
345357
default:
346-
com_port->println(F("err"));
358+
printError();
347359
break;
348360
}
349361
}
350362

351363
void Commander::variable(float* value, char* user_cmd){
352364
if( user_cmd[0] == CMD_SCAN ){
353-
com_port->println(F("var"));
365+
print(F("var"),1);
354366
return;
355367
}
356368
bool GET = user_cmd[0] == '\n';
357369
if(!GET) *value = atof(user_cmd);
358-
printNumber(*value,1);
370+
print(*value,1);
371+
}
372+
373+
374+
void Commander::print(const int number, const bool newline){
375+
if(!com_port) return;
376+
if(newline) com_port->println(number);
377+
else com_port->print(number);
378+
}
379+
void Commander::print(const float number, const bool newline){
380+
if(!com_port) return;
381+
if(newline) com_port->println(number,decimal_places);
382+
else com_port->print(number,decimal_places);
383+
}
384+
void Commander::print(const char* message, const bool newline){
385+
if(!com_port) return;
386+
if(newline) com_port->println(message);
387+
else com_port->print(message);
388+
}
389+
void Commander::print(const __FlashStringHelper *message, const bool newline){
390+
if(!com_port) return;
391+
if(newline) com_port->println(message);
392+
else com_port->print(message);
393+
}
394+
void Commander::print(const char message, const bool newline){
395+
if(!com_port) return;
396+
if(newline) com_port->println(message);
397+
else com_port->print(message);
398+
}
399+
400+
401+
void Commander::verbosePrint(const char* message){
402+
if(verbose) print(message,0);
403+
}
404+
void Commander::verbosePrint(const __FlashStringHelper *message){
405+
if(verbose) print(message,0);
406+
}
407+
void Commander::printError(){
408+
print(F("err"), 1);
359409
}

0 commit comments

Comments
 (0)