Luiz Manoel Sales / Mbed 2 deprecated jorge

bumba

Dependencies:   mbed

main.cpp@0:9d16fef10591, 2019-08-11 (annotated)

Committer:
NeoLewis
Date:
Sun Aug 11 00:57:52 2019 +0000
Revision:
0:9d16fef10591
Child:
1:8aece97d3c78
jesus esse aqui ta comecando a funcionar

Who changed what in which revision?

UserRevisionLine numberNew 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 0:9d16fef10591 11 #define MIN 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 0:9d16fef10591 14 #define MAX 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 0:9d16fef10591 20
NeoLewis 0:9d16fef10591 21
NeoLewis 0:9d16fef10591 22 int i=MID;
NeoLewis 0:9d16fef10591 23 int k=MID;
NeoLewis 0:9d16fef10591 24 int total; //{right, right_diagonal, front, left_diagonal, left} valor atual da leitura
NeoLewis 0:9d16fef10591 25 int measure[5]={0,0,0,0,0}; // vetor que grava a leitura atual
NeoLewis 0:9d16fef10591 26 int tot=0;
NeoLewis 0:9d16fef10591 27
NeoLewis 0:9d16fef10591 28
NeoLewis 0:9d16fef10591 29 //***************************** DEFINICOES - FINAL *****************************
NeoLewis 0:9d16fef10591 30 //################################################################################
NeoLewis 0:9d16fef10591 31 //################################################################################
NeoLewis 0:9d16fef10591 32 //******************************* FUNCOES - INICIO ******************************
NeoLewis 0:9d16fef10591 33 //Funcao que inicializa os motores
NeoLewis 0:9d16fef10591 34 void Motors_Setup()
NeoLewis 0:9d16fef10591 35 {
NeoLewis 0:9d16fef10591 36 Right.period_ms(16);
NeoLewis 0:9d16fef10591 37 Left.period_ms(16);
NeoLewis 0:9d16fef10591 38 Right.pulsewidth_us(MID);
NeoLewis 0:9d16fef10591 39 Left.pulsewidth_us(MID);
NeoLewis 0:9d16fef10591 40 wait(0.1);
NeoLewis 0:9d16fef10591 41 }
NeoLewis 0:9d16fef10591 42 //Função que desloca os motores de acordo com o sinal PPM recebido
NeoLewis 0:9d16fef10591 43 void Drive(int maximo,int minimo,int controle)
NeoLewis 0:9d16fef10591 44 {
NeoLewis 0:9d16fef10591 45 switch(controle)
NeoLewis 0:9d16fef10591 46 {
NeoLewis 0:9d16fef10591 47 case 1:
NeoLewis 0:9d16fef10591 48 Left.pulsewidth_us(minimo);
NeoLewis 0:9d16fef10591 49 Right.pulsewidth_us(minimo);
NeoLewis 0:9d16fef10591 50 break;
NeoLewis 0:9d16fef10591 51 case 2:
NeoLewis 0:9d16fef10591 52 for (;k<=maximo || i<=maximo;k+=50,i+=50)
NeoLewis 0:9d16fef10591 53 {
NeoLewis 0:9d16fef10591 54 Left.pulsewidth_us(k);
NeoLewis 0:9d16fef10591 55 wait(0.05);
NeoLewis 0:9d16fef10591 56 Right.pulsewidth_us(i);
NeoLewis 0:9d16fef10591 57 wait(0.05);
NeoLewis 0:9d16fef10591 58 if (k>MAX)
NeoLewis 0:9d16fef10591 59 {
NeoLewis 0:9d16fef10591 60 k=MAX;
NeoLewis 0:9d16fef10591 61 }
NeoLewis 0:9d16fef10591 62 if (i>MAX)
NeoLewis 0:9d16fef10591 63 {
NeoLewis 0:9d16fef10591 64 i=MAX;
NeoLewis 0:9d16fef10591 65 }
NeoLewis 0:9d16fef10591 66 if (k<MIN)
NeoLewis 0:9d16fef10591 67 {
NeoLewis 0:9d16fef10591 68 k=MIN;
NeoLewis 0:9d16fef10591 69 }
NeoLewis 0:9d16fef10591 70 if (i<MIN)
NeoLewis 0:9d16fef10591 71 {
NeoLewis 0:9d16fef10591 72 i=MIN;
NeoLewis 0:9d16fef10591 73 }
NeoLewis 0:9d16fef10591 74 }
NeoLewis 0:9d16fef10591 75 break;
NeoLewis 0:9d16fef10591 76 case 3:
NeoLewis 0:9d16fef10591 77 for (;k>=minimo || i<=maximo;k-=50,i+=50)
NeoLewis 0:9d16fef10591 78 {
NeoLewis 0:9d16fef10591 79 Left.pulsewidth_us(k);
NeoLewis 0:9d16fef10591 80 wait(0.05);
NeoLewis 0:9d16fef10591 81 Right.pulsewidth_us(i);
NeoLewis 0:9d16fef10591 82 wait(0.05);
NeoLewis 0:9d16fef10591 83 if (k>MAX)
NeoLewis 0:9d16fef10591 84 {
NeoLewis 0:9d16fef10591 85 k=MAX;
NeoLewis 0:9d16fef10591 86 }
NeoLewis 0:9d16fef10591 87 if (i>MAX)
NeoLewis 0:9d16fef10591 88 {
NeoLewis 0:9d16fef10591 89 i=MAX;
NeoLewis 0:9d16fef10591 90 }
NeoLewis 0:9d16fef10591 91 if (k<MIN)
NeoLewis 0:9d16fef10591 92 {
NeoLewis 0:9d16fef10591 93 k=MIN;
NeoLewis 0:9d16fef10591 94 }
NeoLewis 0:9d16fef10591 95 if (i<MIN)
NeoLewis 0:9d16fef10591 96 {
NeoLewis 0:9d16fef10591 97 i=MIN;
NeoLewis 0:9d16fef10591 98 }
NeoLewis 0:9d16fef10591 99 }
NeoLewis 0:9d16fef10591 100
NeoLewis 0:9d16fef10591 101 break;
NeoLewis 0:9d16fef10591 102 case 4:
NeoLewis 0:9d16fef10591 103 for (;k<=maximo || i>=minimo;k+=50,i-=50)
NeoLewis 0:9d16fef10591 104 {
NeoLewis 0:9d16fef10591 105 Left.pulsewidth_us(k);
NeoLewis 0:9d16fef10591 106 wait(0.05);
NeoLewis 0:9d16fef10591 107 Right.pulsewidth_us(i);
NeoLewis 0:9d16fef10591 108 wait(0.05);
NeoLewis 0:9d16fef10591 109 if (k>MAX)
NeoLewis 0:9d16fef10591 110 {
NeoLewis 0:9d16fef10591 111 k=MAX;
NeoLewis 0:9d16fef10591 112 }
NeoLewis 0:9d16fef10591 113 if (i>MAX)
NeoLewis 0:9d16fef10591 114 {
NeoLewis 0:9d16fef10591 115 i=MAX;
NeoLewis 0:9d16fef10591 116 }
NeoLewis 0:9d16fef10591 117 if (k<MIN)
NeoLewis 0:9d16fef10591 118 {
NeoLewis 0:9d16fef10591 119 k=MIN;
NeoLewis 0:9d16fef10591 120 }
NeoLewis 0:9d16fef10591 121 if (i<MIN)
NeoLewis 0:9d16fef10591 122 {
NeoLewis 0:9d16fef10591 123 i=MIN;
NeoLewis 0:9d16fef10591 124 }
NeoLewis 0:9d16fef10591 125 }
NeoLewis 0:9d16fef10591 126
NeoLewis 0:9d16fef10591 127 break;
NeoLewis 0:9d16fef10591 128 case 5:
NeoLewis 0:9d16fef10591 129 for (;k>=minimo || i>=minimo;k-=50,i-=50)
NeoLewis 0:9d16fef10591 130 {
NeoLewis 0:9d16fef10591 131 Left.pulsewidth_us(k);
NeoLewis 0:9d16fef10591 132 wait(0.05);
NeoLewis 0:9d16fef10591 133 Right.pulsewidth_us(i);
NeoLewis 0:9d16fef10591 134 wait(0.05);
NeoLewis 0:9d16fef10591 135 if (k>MAX)
NeoLewis 0:9d16fef10591 136 {
NeoLewis 0:9d16fef10591 137 k=MAX;
NeoLewis 0:9d16fef10591 138 }
NeoLewis 0:9d16fef10591 139 if (i>MAX)
NeoLewis 0:9d16fef10591 140 {
NeoLewis 0:9d16fef10591 141 i=MAX;
NeoLewis 0:9d16fef10591 142 }
NeoLewis 0:9d16fef10591 143 if (k<MIN)
NeoLewis 0:9d16fef10591 144 {
NeoLewis 0:9d16fef10591 145 k=MIN;
NeoLewis 0:9d16fef10591 146 }
NeoLewis 0:9d16fef10591 147 if (i<MIN)
NeoLewis 0:9d16fef10591 148 {
NeoLewis 0:9d16fef10591 149 i=MIN;
NeoLewis 0:9d16fef10591 150 }
NeoLewis 0:9d16fef10591 151 }
NeoLewis 0:9d16fef10591 152
NeoLewis 0:9d16fef10591 153 break;
NeoLewis 0:9d16fef10591 154 default:
NeoLewis 0:9d16fef10591 155 Left.pulsewidth_us(MID);
NeoLewis 0:9d16fef10591 156 Right.pulsewidth_us(MID);
NeoLewis 0:9d16fef10591 157 break;
NeoLewis 0:9d16fef10591 158 }
NeoLewis 0:9d16fef10591 159 }
NeoLewis 0:9d16fef10591 160
NeoLewis 0:9d16fef10591 161 int Search ()
NeoLewis 0:9d16fef10591 162 {
NeoLewis 0:9d16fef10591 163 measure[0] = Sensor1.read();
NeoLewis 0:9d16fef10591 164 measure[1] = Sensor2.read();
NeoLewis 0:9d16fef10591 165 measure[2] = Sensor3.read();
NeoLewis 0:9d16fef10591 166 measure[3] = Sensor4.read();
NeoLewis 0:9d16fef10591 167 measure[4] = Sensor5.read();
NeoLewis 0:9d16fef10591 168
NeoLewis 0:9d16fef10591 169 total = measure[0]*10000 + measure[1]*1000 + measure[2]*100 + measure[3]*10 + measure[4]*1;
NeoLewis 0:9d16fef10591 170 return total;
NeoLewis 0:9d16fef10591 171 }
NeoLewis 0:9d16fef10591 172
NeoLewis 0:9d16fef10591 173 int Action(int total)
NeoLewis 0:9d16fef10591 174 {
NeoLewis 0:9d16fef10591 175 switch(total)
NeoLewis 0:9d16fef10591 176 {
NeoLewis 0:9d16fef10591 177 case 1: //vira para a direita
NeoLewis 0:9d16fef10591 178 Drive(MAX,MIN,d);
NeoLewis 0:9d16fef10591 179 break;
NeoLewis 0:9d16fef10591 180
NeoLewis 0:9d16fef10591 181 case 10: //vira para a direita
NeoLewis 0:9d16fef10591 182 Drive(MAX,MIN,d);
NeoLewis 0:9d16fef10591 183 break;
NeoLewis 0:9d16fef10591 184
NeoLewis 0:9d16fef10591 185 case 11: //vira para a direita
NeoLewis 0:9d16fef10591 186 Drive(MAX,MIN,d);
NeoLewis 0:9d16fef10591 187 break;
NeoLewis 0:9d16fef10591 188
NeoLewis 0:9d16fef10591 189 case 100: //Vai reto
NeoLewis 0:9d16fef10591 190 Drive(MAX,MIN,r);
NeoLewis 0:9d16fef10591 191 break;
NeoLewis 0:9d16fef10591 192
NeoLewis 0:9d16fef10591 193 case 110: //direita com leiruta do sensor da diagonal direita
NeoLewis 0:9d16fef10591 194 Drive(MAX,MIN,d);
NeoLewis 0:9d16fef10591 195 break;
NeoLewis 0:9d16fef10591 196
NeoLewis 0:9d16fef10591 197 case 1000: //vai para a esquerda
NeoLewis 0:9d16fef10591 198 Drive(MAX,MIN,e);
NeoLewis 0:9d16fef10591 199 break;
NeoLewis 0:9d16fef10591 200
NeoLewis 0:9d16fef10591 201 case 1100: //esquerda com leitura do sensor da diagonal esquerda
NeoLewis 0:9d16fef10591 202 Drive(MAX,MIN,e);
NeoLewis 0:9d16fef10591 203 break;
NeoLewis 0:9d16fef10591 204
NeoLewis 0:9d16fef10591 205 case 1110: //vai reto com a leitura dos sensores diagonais e o da frente
NeoLewis 0:9d16fef10591 206 Drive(MAX,MIN,r);
NeoLewis 0:9d16fef10591 207 break;
NeoLewis 0:9d16fef10591 208
NeoLewis 0:9d16fef10591 209 case 10000: //vai para a esquerda
NeoLewis 0:9d16fef10591 210 Drive(MAX,MIN,e);
NeoLewis 0:9d16fef10591 211 break;
NeoLewis 0:9d16fef10591 212
NeoLewis 0:9d16fef10591 213 case 11000: //vai para a esquerda
NeoLewis 0:9d16fef10591 214 Drive(MAX,MIN,e);
NeoLewis 0:9d16fef10591 215 break;
NeoLewis 0:9d16fef10591 216
NeoLewis 0:9d16fef10591 217 case 11111: //leu todos os sensores ao mesmo tempo
NeoLewis 0:9d16fef10591 218 Drive(MAX,MIN,p);
NeoLewis 0:9d16fef10591 219 break;
NeoLewis 0:9d16fef10591 220
NeoLewis 0:9d16fef10591 221 //todos os outros casos que são impossíveis
NeoLewis 0:9d16fef10591 222 default:// Para
NeoLewis 0:9d16fef10591 223 Drive(MAX,MIN,0);
NeoLewis 0:9d16fef10591 224 break;
NeoLewis 0:9d16fef10591 225 }
NeoLewis 0:9d16fef10591 226 return 0;
NeoLewis 0:9d16fef10591 227 }
NeoLewis 0:9d16fef10591 228
NeoLewis 0:9d16fef10591 229 int main()
NeoLewis 0:9d16fef10591 230 {
NeoLewis 0:9d16fef10591 231 while(1)
NeoLewis 0:9d16fef10591 232 {
NeoLewis 0:9d16fef10591 233 tot=Search();
NeoLewis 0:9d16fef10591 234 Action(tot);
NeoLewis 0:9d16fef10591 235 }
NeoLewis 0:9d16fef10591 236 return 0;
NeoLewis 0:9d16fef10591 237 }
NeoLewis 0:9d16fef10591 238