RioBotz / aldebara_com_header

Codigo do aldebaran com headers e dip switch

aldebaran_rc.cpp@0:2228de67a3ef, 2021-03-19 (annotated)

Committer:
pedro_velozo
Date:
Fri Mar 19 16:43:46 2021 +0000
Revision:
0:2228de67a3ef
Codigo do aldebaran com adicao de headers e dip switch;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pedro_velozo 0:2228de67a3ef 1 #include "aldebaran_rc.h"
pedro_velozo 0:2228de67a3ef 2 #include "mbed.h"
pedro_velozo 0:2228de67a3ef 3 #define OFFSET 2
pedro_velozo 0:2228de67a3ef 4
pedro_velozo 0:2228de67a3ef 5 Serial bt(p9, p10);
pedro_velozo 0:2228de67a3ef 6
pedro_velozo 0:2228de67a3ef 7 Timer timer;
pedro_velozo 0:2228de67a3ef 8
pedro_velozo 0:2228de67a3ef 9 DigitalOut st_led(LED1);
pedro_velozo 0:2228de67a3ef 10 DigitalOut th_led(LED2);
pedro_velozo 0:2228de67a3ef 11 DigitalOut mid_led(LED3);
pedro_velozo 0:2228de67a3ef 12
pedro_velozo 0:2228de67a3ef 13 InterruptIn ST(p15);
pedro_velozo 0:2228de67a3ef 14 InterruptIn TH(p14);
pedro_velozo 0:2228de67a3ef 15
pedro_velozo 0:2228de67a3ef 16 PwmOut Right(p21);
pedro_velozo 0:2228de67a3ef 17 PwmOut Left(p22);
pedro_velozo 0:2228de67a3ef 18
pedro_velozo 0:2228de67a3ef 19 int max_st = 1500, max_th = 1500, min_st = 1500, min_th = 1500, mid_st, mid_th;
pedro_velozo 0:2228de67a3ef 20
pedro_velozo 0:2228de67a3ef 21 int RC_value[2];
pedro_velozo 0:2228de67a3ef 22 int RC_start[2];
pedro_velozo 0:2228de67a3ef 23
pedro_velozo 0:2228de67a3ef 24 void ST_High() { RC_start[0] = timer.read_us(); } //start the time }
pedro_velozo 0:2228de67a3ef 25 void ST_Low() { RC_value[0] = timer.read_us() - RC_start[0]; }
pedro_velozo 0:2228de67a3ef 26
pedro_velozo 0:2228de67a3ef 27 void TH_High() { RC_start[1] = timer.read_us(); } //start the time }
pedro_velozo 0:2228de67a3ef 28 void TH_Low() { RC_value[1] = timer.read_us() - RC_start[1]; }
pedro_velozo 0:2228de67a3ef 29
pedro_velozo 0:2228de67a3ef 30 void RC_mode()
pedro_velozo 0:2228de67a3ef 31 {
pedro_velozo 0:2228de67a3ef 32 int st_start, th_start;
pedro_velozo 0:2228de67a3ef 33
pedro_velozo 0:2228de67a3ef 34 timer.start();
pedro_velozo 0:2228de67a3ef 35
pedro_velozo 0:2228de67a3ef 36 ST.fall(&ST_Low);
pedro_velozo 0:2228de67a3ef 37 ST.rise(&ST_High);
pedro_velozo 0:2228de67a3ef 38
pedro_velozo 0:2228de67a3ef 39 TH.fall(&TH_Low);
pedro_velozo 0:2228de67a3ef 40 TH.rise(&TH_High);
pedro_velozo 0:2228de67a3ef 41
pedro_velozo 0:2228de67a3ef 42 Right.period(0.02);
pedro_velozo 0:2228de67a3ef 43 Left.period(0.02);
pedro_velozo 0:2228de67a3ef 44
pedro_velozo 0:2228de67a3ef 45 bt.baud(9600);
pedro_velozo 0:2228de67a3ef 46
pedro_velozo 0:2228de67a3ef 47 st_led = 1;
pedro_velozo 0:2228de67a3ef 48 wait(0.5);
pedro_velozo 0:2228de67a3ef 49 st_led = 0;
pedro_velozo 0:2228de67a3ef 50
pedro_velozo 0:2228de67a3ef 51 st_start = timer.read_us();
pedro_velozo 0:2228de67a3ef 52 while (timer.read_us() - st_start < 5000000)
pedro_velozo 0:2228de67a3ef 53 {
pedro_velozo 0:2228de67a3ef 54 if (RC_value[0] > max_st)
pedro_velozo 0:2228de67a3ef 55 {
pedro_velozo 0:2228de67a3ef 56 max_st = RC_value[0];
pedro_velozo 0:2228de67a3ef 57 }
pedro_velozo 0:2228de67a3ef 58 if (RC_value[0] < min_st)
pedro_velozo 0:2228de67a3ef 59 {
pedro_velozo 0:2228de67a3ef 60 min_st = RC_value[0];
pedro_velozo 0:2228de67a3ef 61 }
pedro_velozo 0:2228de67a3ef 62 }
pedro_velozo 0:2228de67a3ef 63 st_led = 1;
pedro_velozo 0:2228de67a3ef 64 wait(1);
pedro_velozo 0:2228de67a3ef 65 th_led = 1;
pedro_velozo 0:2228de67a3ef 66 wait(0.5);
pedro_velozo 0:2228de67a3ef 67 th_led = 0;
pedro_velozo 0:2228de67a3ef 68 th_start = timer.read_us();
pedro_velozo 0:2228de67a3ef 69 while (timer.read_us() - th_start < 5000000)
pedro_velozo 0:2228de67a3ef 70 {
pedro_velozo 0:2228de67a3ef 71 if (RC_value[1] > max_th)
pedro_velozo 0:2228de67a3ef 72 {
pedro_velozo 0:2228de67a3ef 73 max_th = RC_value[1];
pedro_velozo 0:2228de67a3ef 74 }
pedro_velozo 0:2228de67a3ef 75 if (RC_value[1] < min_th)
pedro_velozo 0:2228de67a3ef 76 {
pedro_velozo 0:2228de67a3ef 77 min_th = RC_value[1];
pedro_velozo 0:2228de67a3ef 78 }
pedro_velozo 0:2228de67a3ef 79 }
pedro_velozo 0:2228de67a3ef 80 th_led = 1;
pedro_velozo 0:2228de67a3ef 81
pedro_velozo 0:2228de67a3ef 82 mid_led = 1;
pedro_velozo 0:2228de67a3ef 83 wait(5);
pedro_velozo 0:2228de67a3ef 84 mid_led = 0;
pedro_velozo 0:2228de67a3ef 85 mid_st = RC_value[0];
pedro_velozo 0:2228de67a3ef 86 mid_th = RC_value[1];
pedro_velozo 0:2228de67a3ef 87 wait(1);
pedro_velozo 0:2228de67a3ef 88 mid_led = 1;
pedro_velozo 0:2228de67a3ef 89
pedro_velozo 0:2228de67a3ef 90 bt.printf("Max-ST: %i Min-ST: %i Max-TH: %i Min-TH: %i\n\r", max_st, min_st, max_th, min_th);
pedro_velozo 0:2228de67a3ef 91
pedro_velozo 0:2228de67a3ef 92 wait(5);
pedro_velozo 0:2228de67a3ef 93 while (1)
pedro_velozo 0:2228de67a3ef 94 {
pedro_velozo 0:2228de67a3ef 95 bt.printf("Left: %i Right: %i\n\r", RC_value[1], RC_value[0]);
pedro_velozo 0:2228de67a3ef 96
pedro_velozo 0:2228de67a3ef 97 Right.pulsewidth_us(mid_st + (mid_st - RC_value[1]) + (mid_th - RC_value[0]));
pedro_velozo 0:2228de67a3ef 98 Left.pulsewidth_us(mid_st + (mid_st - RC_value[1]) - (mid_th - RC_value[0]) - OFFSET);
pedro_velozo 0:2228de67a3ef 99 }
pedro_velozo 0:2228de67a3ef 100 }