Skip to content

Commit 3b8b81c

Browse files
vedderbvedderb
committed
More lisp extensions
1 parent 53286b8 commit 3b8b81c

4 files changed

+160
-59
lines changed

‎lispBM/lispif_vesc_extensions.c

+143-58
Original file line numberDiff line numberDiff line change
@@ -37,25 +37,18 @@
3737

3838
// Helpers
3939

40-
static bool to_float(VALUE t, float *f) {
41-
bool res = false;
42-
43-
if (is_number(t)) {
44-
*f = dec_as_f(t);
45-
res = true;
46-
}
47-
48-
return res;
49-
}
50-
51-
static bool get_arg_float(VALUE *args, UINT argn, float *f) {
52-
if (argn != 1) {
53-
return false;
40+
static bool is_number_all(VALUE *args, UINT argn) {
41+
for (UINT i = 0;i < argn;i++) {
42+
if (!is_number(args[i])) {
43+
return false;
44+
}
5445
}
55-
return to_float(args[0], f);
46+
return true;
5647
}
5748

58-
#define DEC_FLOAT() float f; if (!get_arg_float(args, argn, &f)) {return enc_sym(SYM_EERROR);};
49+
#define CHECK_NUMBER_ALL() if (!is_number_all(args, argn)) {return enc_sym(SYM_EERROR);}
50+
#define CHECK_ARGN(n) if (argn != n) {return enc_sym(SYM_EERROR);}
51+
#define CHECK_ARGN_NUMBER(n) if (argn != n || !is_number_all(args, argn)) {return enc_sym(SYM_EERROR);}
5952

6053
// Various commands
6154

@@ -91,8 +84,8 @@ static VALUE ext_print(VALUE *args, UINT argn) {
9184
}
9285

9386
static VALUE ext_set_servo(VALUE *args, UINT argn) {
94-
DEC_FLOAT()
95-
servo_simple_set_output(f);
87+
CHECK_ARGN_NUMBER(1);
88+
servo_simple_set_output(dec_as_f(args[0]));
9689
return enc_sym(SYM_TRUE);
9790
}
9891

@@ -118,8 +111,8 @@ static VALUE ext_get_vin(VALUE *args, UINT argn) {
118111
}
119112

120113
static VALUE ext_select_motor(VALUE *args, UINT argn) {
121-
DEC_FLOAT()
122-
int i = roundf(f);
114+
CHECK_ARGN_NUMBER(1);
115+
int i = dec_as_i(args[0]);
123116
if (i != 0 && i != 1 && i != 2) {
124117
return enc_sym(SYM_EERROR);
125118
}
@@ -162,24 +155,22 @@ static VALUE ext_get_bms_val(VALUE *args, UINT argn) {
162155
} else if (strcmp(name, "cell_num") == 0) {
163156
res = enc_i(val->cell_num);
164157
} else if (strcmp(name, "v_cell") == 0) {
165-
float f;
166-
if (argn != 2 || !to_float(args[1], &f)) {
158+
if (argn != 2 || !is_number(args[1])) {
167159
return enc_sym(SYM_EERROR);
168160
}
169161

170-
int c = roundf(f);
162+
int c = dec_as_i(args[1]);
171163
if (c < 0 || c >= val->cell_num) {
172164
return enc_sym(SYM_EERROR);
173165
}
174166

175167
res = enc_F(val->v_cell[c]);
176168
} else if (strcmp(name, "bal_state") == 0) {
177-
float f;
178-
if (argn != 2 || !to_float(args[1], &f)) {
169+
if (argn != 2 || !is_number(args[1])) {
179170
return enc_sym(SYM_EERROR);
180171
}
181172

182-
int c = roundf(f);
173+
int c = dec_as_i(args[1]);
183174
if (c < 0 || c >= val->cell_num) {
184175
return enc_sym(SYM_EERROR);
185176
}
@@ -188,12 +179,11 @@ static VALUE ext_get_bms_val(VALUE *args, UINT argn) {
188179
} else if (strcmp(name, "temp_adc_num") == 0) {
189180
res = enc_i(val->temp_adc_num);
190181
} else if (strcmp(name, "temps_adc") == 0) {
191-
float f;
192-
if (argn != 2 || !to_float(args[1], &f)) {
182+
if (argn != 2 || !is_number(args[1])) {
193183
return enc_sym(SYM_EERROR);
194184
}
195185

196-
int c = roundf(f);
186+
int c = dec_as_i(args[1]);
197187
if (c < 0 || c >= val->temp_adc_num) {
198188
return enc_sym(SYM_EERROR);
199189
}
@@ -229,18 +219,16 @@ static VALUE ext_get_bms_val(VALUE *args, UINT argn) {
229219
}
230220

231221
static VALUE ext_get_adc(VALUE *args, UINT argn) {
222+
CHECK_NUMBER_ALL();
223+
232224
if (argn == 0) {
233225
return enc_F(ADC_VOLTS(ADC_IND_EXT));
234226
} else if (argn == 1) {
235-
if (is_number(args[0])) {
236-
INT channel = dec_as_i(args[0]);
237-
if (channel == 0) {
238-
return enc_F(ADC_VOLTS(ADC_IND_EXT));
239-
} else if (channel == 1) {
240-
return enc_F(ADC_VOLTS(ADC_IND_EXT2));
241-
} else {
242-
return enc_sym(SYM_EERROR);
243-
}
227+
INT channel = dec_as_i(args[0]);
228+
if (channel == 0) {
229+
return enc_F(ADC_VOLTS(ADC_IND_EXT));
230+
} else if (channel == 1) {
231+
return enc_F(ADC_VOLTS(ADC_IND_EXT2));
244232
} else {
245233
return enc_sym(SYM_EERROR);
246234
}
@@ -252,56 +240,56 @@ static VALUE ext_get_adc(VALUE *args, UINT argn) {
252240
// Motor set commands
253241

254242
static VALUE ext_set_current(VALUE *args, UINT argn) {
255-
DEC_FLOAT()
256-
mc_interface_set_current(f);
243+
CHECK_ARGN_NUMBER(1);
244+
mc_interface_set_current(dec_as_f(args[0]));
257245
return enc_sym(SYM_TRUE);
258246
}
259247

260248
static VALUE ext_set_current_rel(VALUE *args, UINT argn) {
261-
DEC_FLOAT()
262-
mc_interface_set_current_rel(f);
249+
CHECK_ARGN_NUMBER(1);
250+
mc_interface_set_current_rel(dec_as_f(args[0]));
263251
return enc_sym(SYM_TRUE);
264252
}
265253

266254
static VALUE ext_set_duty(VALUE *args, UINT argn) {
267-
DEC_FLOAT()
268-
mc_interface_set_duty(f);
255+
CHECK_ARGN_NUMBER(1);
256+
mc_interface_set_duty(dec_as_f(args[0]));
269257
return enc_sym(SYM_TRUE);
270258
}
271259

272260
static VALUE ext_set_brake(VALUE *args, UINT argn) {
273-
DEC_FLOAT()
274-
mc_interface_set_brake_current(f);
261+
CHECK_ARGN_NUMBER(1);
262+
mc_interface_set_brake_current(dec_as_f(args[0]));
275263
return enc_sym(SYM_TRUE);
276264
}
277265

278266
static VALUE ext_set_brake_rel(VALUE *args, UINT argn) {
279-
DEC_FLOAT()
280-
mc_interface_set_brake_current_rel(f);
267+
CHECK_ARGN_NUMBER(1);
268+
mc_interface_set_brake_current_rel(dec_as_f(args[0]));
281269
return enc_sym(SYM_TRUE);
282270
}
283271

284272
static VALUE ext_set_handbrake(VALUE *args, UINT argn) {
285-
DEC_FLOAT()
286-
mc_interface_set_handbrake(f);
273+
CHECK_ARGN_NUMBER(1);
274+
mc_interface_set_handbrake(dec_as_f(args[0]));
287275
return enc_sym(SYM_TRUE);
288276
}
289277

290278
static VALUE ext_set_handbrake_rel(VALUE *args, UINT argn) {
291-
DEC_FLOAT()
292-
mc_interface_set_handbrake_rel(f);
279+
CHECK_ARGN_NUMBER(1);
280+
mc_interface_set_handbrake_rel(dec_as_f(args[0]));
293281
return enc_sym(SYM_TRUE);
294282
}
295283

296-
static VALUE ext_set_speed(VALUE *args, UINT argn) {
297-
DEC_FLOAT()
298-
mc_interface_set_pid_speed(f);
284+
static VALUE ext_set_rpm(VALUE *args, UINT argn) {
285+
CHECK_ARGN_NUMBER(1);
286+
mc_interface_set_pid_speed(dec_as_f(args[0]));
299287
return enc_sym(SYM_TRUE);
300288
}
301289

302290
static VALUE ext_set_pos(VALUE *args, UINT argn) {
303-
DEC_FLOAT()
304-
mc_interface_set_pid_pos(f);
291+
CHECK_ARGN_NUMBER(1);
292+
mc_interface_set_pid_pos(dec_as_f(args[0]));
305293
return enc_sym(SYM_TRUE);
306294
}
307295

@@ -362,6 +350,88 @@ static VALUE ext_get_fault(VALUE *args, UINT argn) {
362350
return enc_i(mc_interface_get_fault());
363351
}
364352

353+
// CAN-commands
354+
355+
static VALUE ext_can_current(VALUE *args, UINT argn) {
356+
CHECK_NUMBER_ALL();
357+
358+
if (argn == 2) {
359+
comm_can_set_current(dec_as_i(args[0]), dec_as_f(args[1]));
360+
} else if (argn == 3) {
361+
comm_can_set_current_off_delay(dec_as_i(args[0]), dec_as_f(args[1]), dec_as_f(args[2]));
362+
} else {
363+
return enc_sym(SYM_EERROR);
364+
}
365+
366+
return enc_sym(SYM_TRUE);
367+
}
368+
369+
static VALUE ext_can_current_rel(VALUE *args, UINT argn) {
370+
CHECK_NUMBER_ALL();
371+
372+
if (argn == 2) {
373+
comm_can_set_current_rel(dec_as_i(args[0]), dec_as_f(args[1]));
374+
} else if (argn == 3) {
375+
comm_can_set_current_rel_off_delay(dec_as_i(args[0]), dec_as_f(args[1]), dec_as_f(args[2]));
376+
} else {
377+
return enc_sym(SYM_EERROR);
378+
}
379+
380+
return enc_sym(SYM_TRUE);
381+
}
382+
383+
static VALUE ext_can_duty(VALUE *args, UINT argn) {
384+
CHECK_ARGN_NUMBER(2);
385+
comm_can_set_duty(dec_as_i(args[0]), dec_as_f(args[1]));
386+
return enc_sym(SYM_TRUE);
387+
}
388+
389+
static VALUE ext_can_brake(VALUE *args, UINT argn) {
390+
CHECK_ARGN_NUMBER(2);
391+
comm_can_set_current_brake(dec_as_i(args[0]), dec_as_f(args[1]));
392+
return enc_sym(SYM_TRUE);
393+
}
394+
395+
static VALUE ext_can_brake_rel(VALUE *args, UINT argn) {
396+
CHECK_ARGN_NUMBER(2);
397+
comm_can_set_current_brake_rel(dec_as_i(args[0]), dec_as_f(args[1]));
398+
return enc_sym(SYM_TRUE);
399+
}
400+
401+
static VALUE ext_can_rpm(VALUE *args, UINT argn) {
402+
CHECK_ARGN_NUMBER(2);
403+
comm_can_set_rpm(dec_as_i(args[0]), dec_as_f(args[1]));
404+
return enc_sym(SYM_TRUE);
405+
}
406+
407+
static VALUE ext_can_pos(VALUE *args, UINT argn) {
408+
CHECK_ARGN_NUMBER(2);
409+
comm_can_set_pos(dec_as_i(args[0]), dec_as_f(args[1]));
410+
return enc_sym(SYM_TRUE);
411+
}
412+
413+
// Math
414+
415+
static VALUE ext_sin(VALUE *args, UINT argn) {
416+
CHECK_ARGN_NUMBER(1)
417+
return enc_F(sinf(dec_as_f(args[0])));
418+
}
419+
420+
static VALUE ext_cos(VALUE *args, UINT argn) {
421+
CHECK_ARGN_NUMBER(1)
422+
return enc_F(cosf(dec_as_f(args[0])));
423+
}
424+
425+
static VALUE ext_atan(VALUE *args, UINT argn) {
426+
CHECK_ARGN_NUMBER(1)
427+
return enc_F(atanf(dec_as_f(args[0])));
428+
}
429+
430+
static VALUE ext_pow(VALUE *args, UINT argn) {
431+
CHECK_ARGN_NUMBER(2)
432+
return enc_F(powf(dec_as_f(args[0]), dec_as_f(args[1])));
433+
}
434+
365435
void lispif_load_vesc_extensions(void) {
366436
// Various commands
367437
extensions_add("print", ext_print);
@@ -383,7 +453,7 @@ void lispif_load_vesc_extensions(void) {
383453
extensions_add("set-brake-rel", ext_set_brake_rel);
384454
extensions_add("set-handbrake", ext_set_handbrake);
385455
extensions_add("set-handbrake-rel", ext_set_handbrake_rel);
386-
extensions_add("set-speed", ext_set_speed);
456+
extensions_add("set-rpm", ext_set_rpm);
387457
extensions_add("set-pos", ext_set_pos);
388458

389459
// Motor get commands
@@ -398,4 +468,19 @@ void lispif_load_vesc_extensions(void) {
398468
extensions_add("get-dist", ext_get_dist);
399469
extensions_add("get-batt", ext_get_batt);
400470
extensions_add("get-fault", ext_get_fault);
471+
472+
// CAN-comands
473+
extensions_add("canset-current", ext_can_current);
474+
extensions_add("canset-current-rel", ext_can_current_rel);
475+
extensions_add("canset-duty", ext_can_duty);
476+
extensions_add("canset-brake", ext_can_brake);
477+
extensions_add("canset-brake-rel", ext_can_brake_rel);
478+
extensions_add("canset-rpm", ext_can_rpm);
479+
extensions_add("canset-pos", ext_can_pos);
480+
481+
// Math
482+
extensions_add("sin", ext_sin);
483+
extensions_add("cos", ext_cos);
484+
extensions_add("atan", ext_atan);
485+
extensions_add("pow", ext_pow);
401486
}

‎lispBM/tests/LispLoader.qml

+2-1
Original file line numberDiff line numberDiff line change
@@ -48,8 +48,9 @@ Item {
4848
// mLogReader.openLogFile("example_duty.lisp")
4949
// mLogReader.openLogFile("example_ppm_read.lisp")
5050
// mLogReader.openLogFile("example_control_servo_from_encoder.lisp")
51-
mLogReader.openLogFile("example_control_servo_from_duty.lisp")
51+
// mLogReader.openLogFile("example_control_servo_from_duty.lisp")
5252
// mLogReader.openLogFile("example_print_bms_data.lisp")
53+
mLogReader.openLogFile("example_can_pos_follow.lisp")
5354

5455
// mLogReader.openLogFile("test_math.lisp")
5556

‎lispBM/tests/example_can_pos_follow.lisp

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
(define itcnt 0)
2+
3+
(define f (lambda ()
4+
(progn
5+
(define itcnt (+ itcnt 1))
6+
(canset-pos 124 (get-encoder))
7+
(yield 2000)
8+
(f)
9+
)))
10+
11+
(f)
12+

‎lispBM/tests/example_control_servo_from_duty.lisp

+3
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,11 @@
1010
(set-servo servo-out)
1111
)))
1212

13+
(define itcnt 0)
14+
1315
(define f (lambda ()
1416
(progn
17+
(define itcnt (+ itcnt 1))
1518
(if (> (abs (get-duty)) 0.005) (update-servo) nil)
1619
(yield 10000)
1720
(f)

0 commit comments

Comments
 (0)