RioBotz / aldebara_com_header

Codigo do aldebaran com headers e dip switch

aldebaran_semiauto.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_semiauto.h"
pedro_velozo 0:2228de67a3ef 2 #include "mbed.h"
pedro_velozo 0:2228de67a3ef 3
pedro_velozo 0:2228de67a3ef 4 #define MAX_PPM 2000 // Valor indicando que o motor deve ir para frente com 100% de acordo com o ppm
pedro_velozo 0:2228de67a3ef 5 #define MED_PPM 1500 // Valor indicando que o motor deve ficar parado de acordo com o ppm
pedro_velozo 0:2228de67a3ef 6 #define MIN_PPM 1000 // Valor indicando que o motor deve ir para tras com 100% de acordo com o ppm
pedro_velozo 0:2228de67a3ef 7 #define REVERSE_LEFT 1550 //Signal to make the robot go reverse with the left wheels.
pedro_velozo 0:2228de67a3ef 8 #define FORWARD_LEFT 1550 //Signal to make the robot go forward with the left wheels.
pedro_velozo 0:2228de67a3ef 9 #define REVERSE_RIGHT 1550 //Signal to make the robot go reverse with the left wheels.
pedro_velozo 0:2228de67a3ef 10 #define FORWARD_RIGHT 1550 //Signal to make the robot go forward with the left wheels.
pedro_velozo 0:2228de67a3ef 11 #define BACKWARDS 10
pedro_velozo 0:2228de67a3ef 12
pedro_velozo 0:2228de67a3ef 13 Timer timer;
pedro_velozo 0:2228de67a3ef 14 InterruptIn ST(p15);
pedro_velozo 0:2228de67a3ef 15 InterruptIn TH(p14);
pedro_velozo 0:2228de67a3ef 16
pedro_velozo 0:2228de67a3ef 17 PwmOut Right(p21);
pedro_velozo 0:2228de67a3ef 18 PwmOut Left(p22);
pedro_velozo 0:2228de67a3ef 19
pedro_velozo 0:2228de67a3ef 20 int ch1_start;
pedro_velozo 0:2228de67a3ef 21 int ch2_start;
pedro_velozo 0:2228de67a3ef 22 int ch1_in;
pedro_velozo 0:2228de67a3ef 23 int ch2_in;
pedro_velozo 0:2228de67a3ef 24
pedro_velozo 0:2228de67a3ef 25 void ST_High() { ch1_start = timer.read_us(); } //start the time
pedro_velozo 0:2228de67a3ef 26 void ST_Low() { ch1_in = timer.read_us() - ch1_start; }
pedro_velozo 0:2228de67a3ef 27
pedro_velozo 0:2228de67a3ef 28 void TH_High() { ch2_start = timer.read_us(); } //start the time
pedro_velozo 0:2228de67a3ef 29 void TH_Low() { ch2_in = timer.read_us() - ch2_start; }
pedro_velozo 0:2228de67a3ef 30 //================================================================================
pedro_velozo 0:2228de67a3ef 31 //===================== FAILSAFE - ==============================================
pedro_velozo 0:2228de67a3ef 32 //================================================================================
pedro_velozo 0:2228de67a3ef 33 InterruptIn Failsafe(p12);
pedro_velozo 0:2228de67a3ef 34 DigitalOut Led4(LED4);
pedro_velozo 0:2228de67a3ef 35 int RC_value;
pedro_velozo 0:2228de67a3ef 36 int RC_start;
pedro_velozo 0:2228de67a3ef 37 int failsafe = 0;
pedro_velozo 0:2228de67a3ef 38 #define FAILSAFE_SIGNAL 1400
pedro_velozo 0:2228de67a3ef 39 void intHigh()
pedro_velozo 0:2228de67a3ef 40 {
pedro_velozo 0:2228de67a3ef 41 RC_start = timer.read_us();
pedro_velozo 0:2228de67a3ef 42 }
pedro_velozo 0:2228de67a3ef 43 void intLow()
pedro_velozo 0:2228de67a3ef 44 {
pedro_velozo 0:2228de67a3ef 45 RC_value = timer.read_us() - RC_start;
pedro_velozo 0:2228de67a3ef 46 if (RC_value > FAILSAFE_SIGNAL)
pedro_velozo 0:2228de67a3ef 47 {
pedro_velozo 0:2228de67a3ef 48 failsafe = 0;
pedro_velozo 0:2228de67a3ef 49 } //failsafe não ativado
pedro_velozo 0:2228de67a3ef 50 else
pedro_velozo 0:2228de67a3ef 51 {
pedro_velozo 0:2228de67a3ef 52 failsafe = 1;
pedro_velozo 0:2228de67a3ef 53 } //failsafe ativado
pedro_velozo 0:2228de67a3ef 54 }
pedro_velozo 0:2228de67a3ef 55 //================================================================================
pedro_velozo 0:2228de67a3ef 56 //===================== LINE SENSORS - =========================================
pedro_velozo 0:2228de67a3ef 57 //================================================================================
pedro_velozo 0:2228de67a3ef 58 DigitalOut led1(LED1);
pedro_velozo 0:2228de67a3ef 59 DigitalOut led2(LED2);
pedro_velozo 0:2228de67a3ef 60
pedro_velozo 0:2228de67a3ef 61 #define RightSensor p18
pedro_velozo 0:2228de67a3ef 62 #define LeftSensor p20
pedro_velozo 0:2228de67a3ef 63 #define PARAMETER 0.9
pedro_velozo 0:2228de67a3ef 64
pedro_velozo 0:2228de67a3ef 65 AnalogIn RightRead(RightSensor);
pedro_velozo 0:2228de67a3ef 66 AnalogIn LeftRead(LeftSensor);
pedro_velozo 0:2228de67a3ef 67 float sensor_right;
pedro_velozo 0:2228de67a3ef 68 float sensor_left;
pedro_velozo 0:2228de67a3ef 69 int right_line;
pedro_velozo 0:2228de67a3ef 70 int left_line;
pedro_velozo 0:2228de67a3ef 71 int line;
pedro_velozo 0:2228de67a3ef 72 int READ_SENSORS()
pedro_velozo 0:2228de67a3ef 73 {
pedro_velozo 0:2228de67a3ef 74 sensor_right = RightRead;
pedro_velozo 0:2228de67a3ef 75 sensor_left = LeftRead;
pedro_velozo 0:2228de67a3ef 76 if (sensor_right > PARAMETER)
pedro_velozo 0:2228de67a3ef 77 {
pedro_velozo 0:2228de67a3ef 78 right_line = 1;
pedro_velozo 0:2228de67a3ef 79 } //black
pedro_velozo 0:2228de67a3ef 80 else
pedro_velozo 0:2228de67a3ef 81 {
pedro_velozo 0:2228de67a3ef 82 right_line = 0;
pedro_velozo 0:2228de67a3ef 83 } //white
pedro_velozo 0:2228de67a3ef 84 if (sensor_left > PARAMETER)
pedro_velozo 0:2228de67a3ef 85 {
pedro_velozo 0:2228de67a3ef 86 left_line = 1;
pedro_velozo 0:2228de67a3ef 87 }
pedro_velozo 0:2228de67a3ef 88 else
pedro_velozo 0:2228de67a3ef 89 {
pedro_velozo 0:2228de67a3ef 90 left_line = 0;
pedro_velozo 0:2228de67a3ef 91 }
pedro_velozo 0:2228de67a3ef 92 if (right_line)
pedro_velozo 0:2228de67a3ef 93 {
pedro_velozo 0:2228de67a3ef 94 led1 = 0;
pedro_velozo 0:2228de67a3ef 95 }
pedro_velozo 0:2228de67a3ef 96 else
pedro_velozo 0:2228de67a3ef 97 {
pedro_velozo 0:2228de67a3ef 98 led1 = 1;
pedro_velozo 0:2228de67a3ef 99 }
pedro_velozo 0:2228de67a3ef 100 if (left_line)
pedro_velozo 0:2228de67a3ef 101 {
pedro_velozo 0:2228de67a3ef 102 led2 = 0;
pedro_velozo 0:2228de67a3ef 103 }
pedro_velozo 0:2228de67a3ef 104 else
pedro_velozo 0:2228de67a3ef 105 {
pedro_velozo 0:2228de67a3ef 106 led2 = 1;
pedro_velozo 0:2228de67a3ef 107 }
pedro_velozo 0:2228de67a3ef 108 line = left_line * 10 + right_line;
pedro_velozo 0:2228de67a3ef 109 if (line != 11)
pedro_velozo 0:2228de67a3ef 110 {
pedro_velozo 0:2228de67a3ef 111 return 1;
pedro_velozo 0:2228de67a3ef 112 } //Sensors read something
pedro_velozo 0:2228de67a3ef 113 else
pedro_velozo 0:2228de67a3ef 114 {
pedro_velozo 0:2228de67a3ef 115 return 0;
pedro_velozo 0:2228de67a3ef 116 } //Sensors read nothing
pedro_velozo 0:2228de67a3ef 117 }
pedro_velozo 0:2228de67a3ef 118 void Drive(int LeftSpeed, int RightSpeed)
pedro_velozo 0:2228de67a3ef 119 {
pedro_velozo 0:2228de67a3ef 120 Left.pulsewidth_us(LeftSpeed);
pedro_velozo 0:2228de67a3ef 121 Right.pulsewidth_us(RightSpeed);
pedro_velozo 0:2228de67a3ef 122 }
pedro_velozo 0:2228de67a3ef 123 void calibrate()
pedro_velozo 0:2228de67a3ef 124 {
pedro_velozo 0:2228de67a3ef 125 Left.period_ms(16);
pedro_velozo 0:2228de67a3ef 126 Left.pulsewidth(MAX_PPM);
pedro_velozo 0:2228de67a3ef 127
pedro_velozo 0:2228de67a3ef 128 Right.period_ms(16);
pedro_velozo 0:2228de67a3ef 129 Right.pulsewidth(MAX_PPM);
pedro_velozo 0:2228de67a3ef 130
pedro_velozo 0:2228de67a3ef 131 wait(3);
pedro_velozo 0:2228de67a3ef 132
pedro_velozo 0:2228de67a3ef 133 Drive(MED_PPM, MED_PPM);
pedro_velozo 0:2228de67a3ef 134 wait(3);
pedro_velozo 0:2228de67a3ef 135 }
pedro_velozo 0:2228de67a3ef 136
pedro_velozo 0:2228de67a3ef 137 void semiauto()
pedro_velozo 0:2228de67a3ef 138 {
pedro_velozo 0:2228de67a3ef 139 Drive(MED_PPM, MED_PPM); //Stop motors
pedro_velozo 0:2228de67a3ef 140 for (int i = 0; i < BACKWARDS; i++)
pedro_velozo 0:2228de67a3ef 141 {
pedro_velozo 0:2228de67a3ef 142 Drive(1550, 1450);
pedro_velozo 0:2228de67a3ef 143 }
pedro_velozo 0:2228de67a3ef 144 }
pedro_velozo 0:2228de67a3ef 145
pedro_velozo 0:2228de67a3ef 146 void radio()
pedro_velozo 0:2228de67a3ef 147 {
pedro_velozo 0:2228de67a3ef 148 Drive(ch1_in, ch2_in);
pedro_velozo 0:2228de67a3ef 149 }
pedro_velozo 0:2228de67a3ef 150
pedro_velozo 0:2228de67a3ef 151 void SEMIAUTO_mode()
pedro_velozo 0:2228de67a3ef 152 {
pedro_velozo 0:2228de67a3ef 153 timer.start();
pedro_velozo 0:2228de67a3ef 154 calibrate();
pedro_velozo 0:2228de67a3ef 155 Failsafe.fall(&intLow);
pedro_velozo 0:2228de67a3ef 156 Failsafe.rise(&intHigh);
pedro_velozo 0:2228de67a3ef 157 ST.fall(&ST_Low);
pedro_velozo 0:2228de67a3ef 158 ST.rise(&ST_High);
pedro_velozo 0:2228de67a3ef 159 TH.fall(&TH_Low);
pedro_velozo 0:2228de67a3ef 160 TH.rise(&TH_High);
pedro_velozo 0:2228de67a3ef 161 while (1)
pedro_velozo 0:2228de67a3ef 162 {
pedro_velozo 0:2228de67a3ef 163 if (!failsafe) //Failsafe não ativado
pedro_velozo 0:2228de67a3ef 164 {
pedro_velozo 0:2228de67a3ef 165 Led4 = 1;
pedro_velozo 0:2228de67a3ef 166 if (READ_SENSORS())
pedro_velozo 0:2228de67a3ef 167 {
pedro_velozo 0:2228de67a3ef 168 semiauto(); //line sensors read something
pedro_velozo 0:2228de67a3ef 169 }
pedro_velozo 0:2228de67a3ef 170 else
pedro_velozo 0:2228de67a3ef 171 {
pedro_velozo 0:2228de67a3ef 172 radio();
pedro_velozo 0:2228de67a3ef 173 }
pedro_velozo 0:2228de67a3ef 174 }
pedro_velozo 0:2228de67a3ef 175 else // Failsafe ativado
pedro_velozo 0:2228de67a3ef 176 {
pedro_velozo 0:2228de67a3ef 177 Led4 = 0;
pedro_velozo 0:2228de67a3ef 178 Drive(MED_PPM, MED_PPM);
pedro_velozo 0:2228de67a3ef 179 }
pedro_velozo 0:2228de67a3ef 180 }
pedro_velozo 0:2228de67a3ef 181 }