Codigo do aldebaran com headers e dip switch
Diff: aldebaran_rc.cpp
- Revision:
- 0:2228de67a3ef
diff -r 000000000000 -r 2228de67a3ef aldebaran_rc.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/aldebaran_rc.cpp Fri Mar 19 16:43:46 2021 +0000 @@ -0,0 +1,100 @@ +#include "aldebaran_rc.h" +#include "mbed.h" +#define OFFSET 2 + +Serial bt(p9, p10); + +Timer timer; + +DigitalOut st_led(LED1); +DigitalOut th_led(LED2); +DigitalOut mid_led(LED3); + +InterruptIn ST(p15); +InterruptIn TH(p14); + +PwmOut Right(p21); +PwmOut Left(p22); + +int max_st = 1500, max_th = 1500, min_st = 1500, min_th = 1500, mid_st, mid_th; + +int RC_value[2]; +int RC_start[2]; + +void ST_High() { RC_start[0] = timer.read_us(); } //start the time } +void ST_Low() { RC_value[0] = timer.read_us() - RC_start[0]; } + +void TH_High() { RC_start[1] = timer.read_us(); } //start the time } +void TH_Low() { RC_value[1] = timer.read_us() - RC_start[1]; } + +void RC_mode() +{ + int st_start, th_start; + + timer.start(); + + ST.fall(&ST_Low); + ST.rise(&ST_High); + + TH.fall(&TH_Low); + TH.rise(&TH_High); + + Right.period(0.02); + Left.period(0.02); + + bt.baud(9600); + + st_led = 1; + wait(0.5); + st_led = 0; + + st_start = timer.read_us(); + while (timer.read_us() - st_start < 5000000) + { + if (RC_value[0] > max_st) + { + max_st = RC_value[0]; + } + if (RC_value[0] < min_st) + { + min_st = RC_value[0]; + } + } + st_led = 1; + wait(1); + th_led = 1; + wait(0.5); + th_led = 0; + th_start = timer.read_us(); + while (timer.read_us() - th_start < 5000000) + { + if (RC_value[1] > max_th) + { + max_th = RC_value[1]; + } + if (RC_value[1] < min_th) + { + min_th = RC_value[1]; + } + } + th_led = 1; + + mid_led = 1; + wait(5); + mid_led = 0; + mid_st = RC_value[0]; + mid_th = RC_value[1]; + wait(1); + mid_led = 1; + + bt.printf("Max-ST: %i Min-ST: %i Max-TH: %i Min-TH: %i\n\r", max_st, min_st, max_th, min_th); + + wait(5); + while (1) + { + bt.printf("Left: %i Right: %i\n\r", RC_value[1], RC_value[0]); + + Right.pulsewidth_us(mid_st + (mid_st - RC_value[1]) + (mid_th - RC_value[0])); + Left.pulsewidth_us(mid_st + (mid_st - RC_value[1]) - (mid_th - RC_value[0]) - OFFSET); + } +}