Luiz Manoel Sales
bumba
main.cpp@1:8aece97d3c78, 2019-08-12 (annotated)
- Committer:
- NeoLewis
- Date:
- Mon Aug 12 12:40:14 2019 +0000
- Revision:
- 1:8aece97d3c78
- Parent:
- 0:9d16fef10591
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| NeoLewis | 0:9d16fef10591 | 1 | #include "mbed.h" |
| NeoLewis | 0:9d16fef10591 | 2 | PwmOut Left(p25); //Definição do motor da esquerda |
| NeoLewis | 0:9d16fef10591 | 3 | PwmOut Right(p23); //Definicao do motor da direita |
| NeoLewis | 0:9d16fef10591 | 4 | DigitalIn Sensor1 (p17);// Sensor digital localizado na esquerda do robo |
| NeoLewis | 0:9d16fef10591 | 5 | DigitalIn Sensor2 (p15);//Sensor digital localizado na diagonal esquerda do robo |
| NeoLewis | 0:9d16fef10591 | 6 | DigitalIn Sensor3 (p13);//Sensor digital localizado na frente do robo |
| NeoLewis | 0:9d16fef10591 | 7 | DigitalIn Sensor4 (p11); //Sensor digital localizado na diagonal direita do robo |
| NeoLewis | 0:9d16fef10591 | 8 | DigitalIn Sensor5 (p9); //Sensor digital localizado na direita do robo |
| NeoLewis | 0:9d16fef10591 | 9 | |
| NeoLewis | 0:9d16fef10591 | 10 | |
| NeoLewis | 1:8aece97d3c78 | 11 | #define MIN 1260//1060 //velocidade maxima para tras |
| NeoLewis | 0:9d16fef10591 | 12 | #define MID ((MAX+MIN)/2) //valor para parar |
| NeoLewis | 0:9d16fef10591 | 13 | #define BUS 12 //velocidade de busca |
| NeoLewis | 1:8aece97d3c78 | 14 | #define MAX 1660//1860 //velocidade maxima para frente |
| NeoLewis | 0:9d16fef10591 | 15 | #define r 2 |
| NeoLewis | 0:9d16fef10591 | 16 | #define e 3 |
| NeoLewis | 0:9d16fef10591 | 17 | #define d 4 |
| NeoLewis | 0:9d16fef10591 | 18 | #define p 9 |
| NeoLewis | 0:9d16fef10591 | 19 | #define b 5 |
| NeoLewis | 1:8aece97d3c78 | 20 | #define PARAMETER 0.600 |
| NeoLewis | 0:9d16fef10591 | 21 | |
| NeoLewis | 0:9d16fef10591 | 22 | |
| NeoLewis | 0:9d16fef10591 | 23 | int i=MID; |
| NeoLewis | 0:9d16fef10591 | 24 | int k=MID; |
| NeoLewis | 0:9d16fef10591 | 25 | int total; //{right, right_diagonal, front, left_diagonal, left} valor atual da leitura |
| NeoLewis | 1:8aece97d3c78 | 26 | int measure[5]; // vetor que grava a leitura atual |
| NeoLewis | 1:8aece97d3c78 | 27 | int tot; |
| NeoLewis | 1:8aece97d3c78 | 28 | int in=20; |
| NeoLewis | 0:9d16fef10591 | 29 | |
| NeoLewis | 0:9d16fef10591 | 30 | //***************************** DEFINICOES - FINAL ***************************** |
| NeoLewis | 0:9d16fef10591 | 31 | //################################################################################ |
| NeoLewis | 0:9d16fef10591 | 32 | //################################################################################ |
| NeoLewis | 0:9d16fef10591 | 33 | //******************************* FUNCOES - INICIO ****************************** |
| NeoLewis | 0:9d16fef10591 | 34 | //Funcao que inicializa os motores |
| NeoLewis | 0:9d16fef10591 | 35 | void Motors_Setup() |
| NeoLewis | 0:9d16fef10591 | 36 | { |
| NeoLewis | 0:9d16fef10591 | 37 | Right.period_ms(16); |
| NeoLewis | 0:9d16fef10591 | 38 | Left.period_ms(16); |
| NeoLewis | 0:9d16fef10591 | 39 | Right.pulsewidth_us(MID); |
| NeoLewis | 0:9d16fef10591 | 40 | Left.pulsewidth_us(MID); |
| NeoLewis | 0:9d16fef10591 | 41 | wait(0.1); |
| NeoLewis | 0:9d16fef10591 | 42 | } |
| NeoLewis | 0:9d16fef10591 | 43 | //Função que desloca os motores de acordo com o sinal PPM recebido |
| NeoLewis | 0:9d16fef10591 | 44 | void Drive(int maximo,int minimo,int controle) |
| NeoLewis | 0:9d16fef10591 | 45 | { |
| NeoLewis | 0:9d16fef10591 | 46 | switch(controle) |
| NeoLewis | 0:9d16fef10591 | 47 | { |
| NeoLewis | 0:9d16fef10591 | 48 | case 1: |
| NeoLewis | 1:8aece97d3c78 | 49 | Left.pulsewidth_us(minimo); |
| NeoLewis | 1:8aece97d3c78 | 50 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 51 | Right.pulsewidth_us(minimo); |
| NeoLewis | 1:8aece97d3c78 | 52 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 53 | break; |
| NeoLewis | 0:9d16fef10591 | 54 | case 2: |
| NeoLewis | 1:8aece97d3c78 | 55 | //Right.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 56 | //Left.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 57 | for (;k<=maximo && i<=maximo;k+=in,i+=in) |
| NeoLewis | 0:9d16fef10591 | 58 | { |
| NeoLewis | 0:9d16fef10591 | 59 | Left.pulsewidth_us(k); |
| NeoLewis | 1:8aece97d3c78 | 60 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 61 | Right.pulsewidth_us(i); |
| NeoLewis | 1:8aece97d3c78 | 62 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 63 | if (k>MAX) |
| NeoLewis | 0:9d16fef10591 | 64 | { |
| NeoLewis | 0:9d16fef10591 | 65 | k=MAX; |
| NeoLewis | 0:9d16fef10591 | 66 | } |
| NeoLewis | 0:9d16fef10591 | 67 | if (i>MAX) |
| NeoLewis | 0:9d16fef10591 | 68 | { |
| NeoLewis | 0:9d16fef10591 | 69 | i=MAX; |
| NeoLewis | 0:9d16fef10591 | 70 | } |
| NeoLewis | 0:9d16fef10591 | 71 | if (k<MIN) |
| NeoLewis | 0:9d16fef10591 | 72 | { |
| NeoLewis | 0:9d16fef10591 | 73 | k=MIN; |
| NeoLewis | 0:9d16fef10591 | 74 | } |
| NeoLewis | 0:9d16fef10591 | 75 | if (i<MIN) |
| NeoLewis | 0:9d16fef10591 | 76 | { |
| NeoLewis | 0:9d16fef10591 | 77 | i=MIN; |
| NeoLewis | 0:9d16fef10591 | 78 | } |
| NeoLewis | 0:9d16fef10591 | 79 | } |
| NeoLewis | 0:9d16fef10591 | 80 | break; |
| NeoLewis | 0:9d16fef10591 | 81 | case 3: |
| NeoLewis | 1:8aece97d3c78 | 82 | //Right.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 83 | //Left.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 84 | for (;k>=minimo || i<=maximo;k-=in,i+=in) |
| NeoLewis | 0:9d16fef10591 | 85 | { |
| NeoLewis | 0:9d16fef10591 | 86 | Left.pulsewidth_us(k); |
| NeoLewis | 1:8aece97d3c78 | 87 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 88 | Right.pulsewidth_us(i); |
| NeoLewis | 1:8aece97d3c78 | 89 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 90 | if (k>MAX) |
| NeoLewis | 0:9d16fef10591 | 91 | { |
| NeoLewis | 0:9d16fef10591 | 92 | k=MAX; |
| NeoLewis | 0:9d16fef10591 | 93 | } |
| NeoLewis | 0:9d16fef10591 | 94 | if (i>MAX) |
| NeoLewis | 0:9d16fef10591 | 95 | { |
| NeoLewis | 0:9d16fef10591 | 96 | i=MAX; |
| NeoLewis | 0:9d16fef10591 | 97 | } |
| NeoLewis | 0:9d16fef10591 | 98 | if (k<MIN) |
| NeoLewis | 0:9d16fef10591 | 99 | { |
| NeoLewis | 0:9d16fef10591 | 100 | k=MIN; |
| NeoLewis | 0:9d16fef10591 | 101 | } |
| NeoLewis | 0:9d16fef10591 | 102 | if (i<MIN) |
| NeoLewis | 0:9d16fef10591 | 103 | { |
| NeoLewis | 0:9d16fef10591 | 104 | i=MIN; |
| NeoLewis | 0:9d16fef10591 | 105 | } |
| NeoLewis | 0:9d16fef10591 | 106 | } |
| NeoLewis | 0:9d16fef10591 | 107 | |
| NeoLewis | 0:9d16fef10591 | 108 | break; |
| NeoLewis | 0:9d16fef10591 | 109 | case 4: |
| NeoLewis | 1:8aece97d3c78 | 110 | //Right.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 111 | //Left.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 112 | for (;k<=maximo || i>=minimo;k+=in,i-=in) |
| NeoLewis | 0:9d16fef10591 | 113 | { |
| NeoLewis | 0:9d16fef10591 | 114 | Left.pulsewidth_us(k); |
| NeoLewis | 1:8aece97d3c78 | 115 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 116 | Right.pulsewidth_us(i); |
| NeoLewis | 1:8aece97d3c78 | 117 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 118 | if (k>MAX) |
| NeoLewis | 0:9d16fef10591 | 119 | { |
| NeoLewis | 0:9d16fef10591 | 120 | k=MAX; |
| NeoLewis | 0:9d16fef10591 | 121 | } |
| NeoLewis | 0:9d16fef10591 | 122 | if (i>MAX) |
| NeoLewis | 0:9d16fef10591 | 123 | { |
| NeoLewis | 0:9d16fef10591 | 124 | i=MAX; |
| NeoLewis | 0:9d16fef10591 | 125 | } |
| NeoLewis | 0:9d16fef10591 | 126 | if (k<MIN) |
| NeoLewis | 0:9d16fef10591 | 127 | { |
| NeoLewis | 0:9d16fef10591 | 128 | k=MIN; |
| NeoLewis | 0:9d16fef10591 | 129 | } |
| NeoLewis | 0:9d16fef10591 | 130 | if (i<MIN) |
| NeoLewis | 0:9d16fef10591 | 131 | { |
| NeoLewis | 0:9d16fef10591 | 132 | i=MIN; |
| NeoLewis | 0:9d16fef10591 | 133 | } |
| NeoLewis | 0:9d16fef10591 | 134 | } |
| NeoLewis | 0:9d16fef10591 | 135 | |
| NeoLewis | 0:9d16fef10591 | 136 | break; |
| NeoLewis | 0:9d16fef10591 | 137 | case 5: |
| NeoLewis | 1:8aece97d3c78 | 138 | //Right.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 139 | //Left.period_ms(16); |
| NeoLewis | 1:8aece97d3c78 | 140 | for (;k>=minimo || i>=minimo;k-=in,i-=in) |
| NeoLewis | 0:9d16fef10591 | 141 | { |
| NeoLewis | 0:9d16fef10591 | 142 | Left.pulsewidth_us(k); |
| NeoLewis | 1:8aece97d3c78 | 143 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 144 | Right.pulsewidth_us(i); |
| NeoLewis | 1:8aece97d3c78 | 145 | wait(0.02); |
| NeoLewis | 0:9d16fef10591 | 146 | if (k>MAX) |
| NeoLewis | 0:9d16fef10591 | 147 | { |
| NeoLewis | 0:9d16fef10591 | 148 | k=MAX; |
| NeoLewis | 0:9d16fef10591 | 149 | } |
| NeoLewis | 0:9d16fef10591 | 150 | if (i>MAX) |
| NeoLewis | 0:9d16fef10591 | 151 | { |
| NeoLewis | 0:9d16fef10591 | 152 | i=MAX; |
| NeoLewis | 0:9d16fef10591 | 153 | } |
| NeoLewis | 0:9d16fef10591 | 154 | if (k<MIN) |
| NeoLewis | 0:9d16fef10591 | 155 | { |
| NeoLewis | 0:9d16fef10591 | 156 | k=MIN; |
| NeoLewis | 0:9d16fef10591 | 157 | } |
| NeoLewis | 0:9d16fef10591 | 158 | if (i<MIN) |
| NeoLewis | 0:9d16fef10591 | 159 | { |
| NeoLewis | 0:9d16fef10591 | 160 | i=MIN; |
| NeoLewis | 0:9d16fef10591 | 161 | } |
| NeoLewis | 0:9d16fef10591 | 162 | } |
| NeoLewis | 0:9d16fef10591 | 163 | |
| NeoLewis | 0:9d16fef10591 | 164 | break; |
| NeoLewis | 0:9d16fef10591 | 165 | default: |
| NeoLewis | 0:9d16fef10591 | 166 | Left.pulsewidth_us(MID); |
| NeoLewis | 0:9d16fef10591 | 167 | Right.pulsewidth_us(MID); |
| NeoLewis | 0:9d16fef10591 | 168 | break; |
| NeoLewis | 0:9d16fef10591 | 169 | } |
| NeoLewis | 0:9d16fef10591 | 170 | } |
| NeoLewis | 0:9d16fef10591 | 171 | |
| NeoLewis | 1:8aece97d3c78 | 172 | void Aceleration(int esq, int dir) |
| NeoLewis | 1:8aece97d3c78 | 173 | { |
| NeoLewis | 1:8aece97d3c78 | 174 | Left.pulsewidth_us(esq); |
| NeoLewis | 1:8aece97d3c78 | 175 | wait(0.02); |
| NeoLewis | 1:8aece97d3c78 | 176 | Right.pulsewidth_us(dir); |
| NeoLewis | 1:8aece97d3c78 | 177 | wait(0.02); |
| NeoLewis | 1:8aece97d3c78 | 178 | } |
| NeoLewis | 1:8aece97d3c78 | 179 | |
| NeoLewis | 0:9d16fef10591 | 180 | int Search () |
| NeoLewis | 0:9d16fef10591 | 181 | { |
| NeoLewis | 0:9d16fef10591 | 182 | measure[0] = Sensor1.read(); |
| NeoLewis | 0:9d16fef10591 | 183 | measure[1] = Sensor2.read(); |
| NeoLewis | 0:9d16fef10591 | 184 | measure[2] = Sensor3.read(); |
| NeoLewis | 0:9d16fef10591 | 185 | measure[3] = Sensor4.read(); |
| NeoLewis | 0:9d16fef10591 | 186 | measure[4] = Sensor5.read(); |
| NeoLewis | 0:9d16fef10591 | 187 | |
| NeoLewis | 0:9d16fef10591 | 188 | total = measure[0]*10000 + measure[1]*1000 + measure[2]*100 + measure[3]*10 + measure[4]*1; |
| NeoLewis | 0:9d16fef10591 | 189 | return total; |
| NeoLewis | 0:9d16fef10591 | 190 | } |
| NeoLewis | 0:9d16fef10591 | 191 | |
| NeoLewis | 0:9d16fef10591 | 192 | int Action(int total) |
| NeoLewis | 0:9d16fef10591 | 193 | { |
| NeoLewis | 0:9d16fef10591 | 194 | switch(total) |
| NeoLewis | 0:9d16fef10591 | 195 | { |
| NeoLewis | 0:9d16fef10591 | 196 | case 1: //vira para a direita |
| NeoLewis | 0:9d16fef10591 | 197 | Drive(MAX,MIN,d); |
| NeoLewis | 1:8aece97d3c78 | 198 | Aceleration(MAX,MIN); |
| NeoLewis | 0:9d16fef10591 | 199 | break; |
| NeoLewis | 0:9d16fef10591 | 200 | |
| NeoLewis | 0:9d16fef10591 | 201 | case 10: //vira para a direita |
| NeoLewis | 1:8aece97d3c78 | 202 | Drive(MAX,MIN,d); |
| NeoLewis | 1:8aece97d3c78 | 203 | Aceleration(MAX,MIN); |
| NeoLewis | 0:9d16fef10591 | 204 | break; |
| NeoLewis | 0:9d16fef10591 | 205 | |
| NeoLewis | 0:9d16fef10591 | 206 | case 11: //vira para a direita |
| NeoLewis | 0:9d16fef10591 | 207 | Drive(MAX,MIN,d); |
| NeoLewis | 1:8aece97d3c78 | 208 | Aceleration(MAX,MIN); |
| NeoLewis | 0:9d16fef10591 | 209 | break; |
| NeoLewis | 0:9d16fef10591 | 210 | |
| NeoLewis | 0:9d16fef10591 | 211 | case 100: //Vai reto |
| NeoLewis | 0:9d16fef10591 | 212 | Drive(MAX,MIN,r); |
| NeoLewis | 1:8aece97d3c78 | 213 | Aceleration(MAX,MAX); |
| NeoLewis | 0:9d16fef10591 | 214 | break; |
| NeoLewis | 0:9d16fef10591 | 215 | |
| NeoLewis | 0:9d16fef10591 | 216 | case 110: //direita com leiruta do sensor da diagonal direita |
| NeoLewis | 0:9d16fef10591 | 217 | Drive(MAX,MIN,d); |
| NeoLewis | 1:8aece97d3c78 | 218 | Aceleration(MAX,MAX); |
| NeoLewis | 0:9d16fef10591 | 219 | break; |
| NeoLewis | 0:9d16fef10591 | 220 | |
| NeoLewis | 0:9d16fef10591 | 221 | case 1000: //vai para a esquerda |
| NeoLewis | 0:9d16fef10591 | 222 | Drive(MAX,MIN,e); |
| NeoLewis | 1:8aece97d3c78 | 223 | Aceleration(MIN,MAX); |
| NeoLewis | 0:9d16fef10591 | 224 | break; |
| NeoLewis | 0:9d16fef10591 | 225 | |
| NeoLewis | 0:9d16fef10591 | 226 | case 1100: //esquerda com leitura do sensor da diagonal esquerda |
| NeoLewis | 0:9d16fef10591 | 227 | Drive(MAX,MIN,e); |
| NeoLewis | 1:8aece97d3c78 | 228 | Aceleration(MIN,MAX); |
| NeoLewis | 0:9d16fef10591 | 229 | break; |
| NeoLewis | 0:9d16fef10591 | 230 | |
| NeoLewis | 0:9d16fef10591 | 231 | case 1110: //vai reto com a leitura dos sensores diagonais e o da frente |
| NeoLewis | 0:9d16fef10591 | 232 | Drive(MAX,MIN,r); |
| NeoLewis | 1:8aece97d3c78 | 233 | Aceleration(MAX,MAX); |
| NeoLewis | 0:9d16fef10591 | 234 | break; |
| NeoLewis | 0:9d16fef10591 | 235 | |
| NeoLewis | 0:9d16fef10591 | 236 | case 10000: //vai para a esquerda |
| NeoLewis | 0:9d16fef10591 | 237 | Drive(MAX,MIN,e); |
| NeoLewis | 1:8aece97d3c78 | 238 | Aceleration(MIN,MAX); |
| NeoLewis | 0:9d16fef10591 | 239 | break; |
| NeoLewis | 0:9d16fef10591 | 240 | |
| NeoLewis | 0:9d16fef10591 | 241 | case 11000: //vai para a esquerda |
| NeoLewis | 0:9d16fef10591 | 242 | Drive(MAX,MIN,e); |
| NeoLewis | 1:8aece97d3c78 | 243 | Aceleration(MIN,MAX); |
| NeoLewis | 0:9d16fef10591 | 244 | break; |
| NeoLewis | 0:9d16fef10591 | 245 | |
| NeoLewis | 0:9d16fef10591 | 246 | case 11111: //leu todos os sensores ao mesmo tempo |
| NeoLewis | 0:9d16fef10591 | 247 | Drive(MAX,MIN,p); |
| NeoLewis | 1:8aece97d3c78 | 248 | Aceleration(MID,MID); |
| NeoLewis | 0:9d16fef10591 | 249 | break; |
| NeoLewis | 0:9d16fef10591 | 250 | |
| NeoLewis | 0:9d16fef10591 | 251 | //todos os outros casos que são impossíveis |
| NeoLewis | 0:9d16fef10591 | 252 | default:// Para |
| NeoLewis | 0:9d16fef10591 | 253 | Drive(MAX,MIN,0); |
| NeoLewis | 0:9d16fef10591 | 254 | break; |
| NeoLewis | 0:9d16fef10591 | 255 | } |
| NeoLewis | 0:9d16fef10591 | 256 | return 0; |
| NeoLewis | 0:9d16fef10591 | 257 | } |
| NeoLewis | 0:9d16fef10591 | 258 | |
| NeoLewis | 0:9d16fef10591 | 259 | int main() |
| NeoLewis | 0:9d16fef10591 | 260 | { |
| NeoLewis | 0:9d16fef10591 | 261 | while(1) |
| NeoLewis | 0:9d16fef10591 | 262 | { |
| NeoLewis | 0:9d16fef10591 | 263 | tot=Search(); |
| NeoLewis | 0:9d16fef10591 | 264 | Action(tot); |
| NeoLewis | 0:9d16fef10591 | 265 | } |
| NeoLewis | 1:8aece97d3c78 | 266 | return 0; |
| NeoLewis | 1:8aece97d3c78 | 267 | } |
| NeoLewis | 0:9d16fef10591 | 268 |