SHOW:
|
|
- or go back to the newest paste.
| 1 | /********************************* | |
| 2 | Autor: David Lucas | |
| 3 | Programa: Projeto carro robot 1.0 | |
| 4 | Data: 20/10/2010 | |
| 5 | Arduino: DUEMILANOVE | |
| 6 | Inc. All Rights Reserved | |
| 7 | ||
| 8 | IN1,IN2--Motor A(roda direita) | |
| 9 | IN3,IN4--Motor B(roda esquerda) | |
| 10 | ||
| 11 | L298N Arduino Code | |
| 12 | IN1-------11-------pinRF | |
| 13 | IN2-------10-------pinRB | |
| 14 | IN3-------9--------pinLF | |
| 15 | IN4-------6--------pinLB | |
| 16 | ||
| 17 | IN1 IN2 | |
| 18 | 1 0 Motor A no sentido anti-horário | |
| 19 | 0 1 Motor A sentido horário | |
| 20 | 1 1 stop | |
| 21 | ||
| 22 | IN3 IN4 | |
| 23 | 0 1 Motor B no sentido horário | |
| 24 | 1 0 Motor B no sentido anti-horário | |
| 25 | 1 1 stop | |
| 26 | ||
| 27 | pinRF pinRB pinLF pinLB | |
| 28 | IN1 IN2 IN3 IN4 Car | |
| 29 | 1 0 0 1 advance | |
| 30 | 0 1 1 0 back | |
| 31 | 1 1 1 1 stop | |
| 32 | 1 0 1 1 Única roda vire à esquerda | |
| 33 | 1 1 0 1 Única roda para a direita | |
| 34 | 1 0 1 0 Roda dupla vire à esquerda | |
| 35 | 0 1 0 1 Roda dupla para a direita | |
| 36 | ||
| 37 | ||
| 38 | ||
| 39 | ||
| 40 | ||
| 41 | L = left | |
| 42 | R = right | |
| 43 | F = advance | |
| 44 | B = back | |
| 45 | */ | |
| 46 | #include <Servo.h> | |
| 47 | int pinLB=6; // volta esquerda | |
| 48 | int pinLF=9; // frente de esquerda | |
| 49 | ||
| 50 | int pinRB=10; // Volta direita | |
| 51 | int pinRF=11; // frente de esquerda | |
| 52 | ||
| 53 | int inputPin = A0; // ultrasom echo | |
| 54 | int outputPin =A1; // ultrasom trig | |
| 55 | ||
| 56 | int Fspeedd = 0; // distância da frente | |
| 57 | int Rspeedd = 0; // distância certa | |
| 58 | int Lspeedd = 0; // distância à esquerda | |
| 59 | int directionn = 0; // Determinar a direção de voltas de carro | |
| 60 | Servo myservo; // myservo | |
| 61 | int delay_time = 250; // Servo motor de direção estável | |
| 62 | ||
| 63 | int Fgo = 8; // avança | |
| 64 | int Rgo = 6; // vire à direita | |
| 65 | int Lgo = 4; // vire à esquerda | |
| 66 | int Bgo = 2; // volta | |
| 67 | ||
| 68 | void setup() | |
| 69 | {
| |
| 70 | pinMode(pinLB,OUTPUT); // pin 6 (PWM) | |
| 71 | pinMode(pinLF,OUTPUT); // pin 9 (PWM) | |
| 72 | pinMode(pinRB,OUTPUT); // pin 10 (PWM) | |
| 73 | pinMode(pinRF,OUTPUT); // pin 11 (PWM) | |
| 74 | ||
| 75 | pinMode(inputPin, INPUT); // Defini entrada pino de ultra-som | |
| 76 | pinMode(outputPin, OUTPUT); // Defini o pino de saída de ultra-som | |
| 77 | ||
| 78 | myservo.attach(5); // Defini o servo motor saída pin5 (PWM) | |
| 79 | } | |
| 80 | void advance(int a) // avanço | |
| 81 | { //No ponto médio das duas rodas como referência
| |
| 82 | digitalWrite(pinRB,LOW); //avanço da roda direita | |
| 83 | digitalWrite(pinRF,HIGH); | |
| 84 | digitalWrite(pinLB,HIGH); //avanço da roda esquerda | |
| 85 | digitalWrite(pinLF,LOW); | |
| 86 | delay(a); | |
| 87 | } | |
| 88 | void right(int b) //Vire à direita (única roda) | |
| 89 | {
| |
| 90 | digitalWrite(pinRB,HIGH); //Para Direita | |
| 91 | digitalWrite(pinRF,HIGH); | |
| 92 | digitalWrite(pinLB,HIGH); //avanço esquerdo | |
| 93 | digitalWrite(pinLF,LOW); | |
| 94 | delay(b); | |
| 95 | } | |
| 96 | void left(int c) //Vire à esquerda (a única roda) | |
| 97 | {
| |
| 98 | digitalWrite(pinRB,LOW); //avanço da roda direita | |
| 99 | digitalWrite(pinRF,HIGH); | |
| 100 | digitalWrite(pinLB,HIGH); //para esquerda | |
| 101 | digitalWrite(pinLF,HIGH); | |
| 102 | delay(c); | |
| 103 | } | |
| 104 | void turnR(int d) //Vire à direita (rodado duplo) | |
| 105 | {
| |
| 106 | digitalWrite(pinRB,HIGH); //roda direita volta | |
| 107 | digitalWrite(pinRF,LOW); | |
| 108 | digitalWrite(pinLB,HIGH); //avanço da roda esquerda | |
| 109 | digitalWrite(pinLF,LOW); | |
| 110 | delay(d); | |
| 111 | } | |
| 112 | void turnL(int e) //Vire à esquerda (rodado duplo) | |
| 113 | {
| |
| 114 | digitalWrite(pinRB,LOW); //avanço da roda direita | |
| 115 | digitalWrite(pinRF,HIGH); | |
| 116 | digitalWrite(pinLB,LOW); //roda esquerda volta | |
| 117 | digitalWrite(pinLF,HIGH); | |
| 118 | delay(e); | |
| 119 | } | |
| 120 | void stopp(int f) //Pare | |
| 121 | {
| |
| 122 | digitalWrite(pinRB,HIGH); | |
| 123 | digitalWrite(pinRF,HIGH); | |
| 124 | digitalWrite(pinLB,HIGH); | |
| 125 | digitalWrite(pinLF,HIGH); | |
| 126 | delay(f); | |
| 127 | } | |
| 128 | void back(int g) //Volta | |
| 129 | {
| |
| 130 | digitalWrite(pinRB,HIGH); //roda direita volta | |
| 131 | digitalWrite(pinRF,LOW); | |
| 132 | digitalWrite(pinLB,LOW); //roda esquerda volta | |
| 133 | digitalWrite(pinLF,HIGH); | |
| 134 | delay(g); | |
| 135 | } | |
| 136 | void detection() //Três ângulos de medição(2.90.178) | |
| 137 | {
| |
| 138 | myservo.write(90); //medida de distância na frente | |
| 139 | delay(delay_time); // Esperando o servo, motor estável | |
| 140 | ask_pin_F(); // Ler a distância da frente | |
| 141 | ||
| 142 | if(Fspeedd < 20) // Se a distância for inferior a 20cm na frente | |
| 143 | {
| |
| 144 | stopp(1); // limpar a saída, parada do motor | |
| 145 | myservo.write(178); //Meça a distância à esquerda | |
| 146 | delay(delay_time); | |
| 147 | ask_pin_L(); | |
| 148 | ||
| 149 | myservo.write(2); //distância medida certa | |
| 150 | delay(delay_time); | |
| 151 | ask_pin_R(); | |
| 152 | ||
| 153 | if(Lspeedd > Rspeedd) //comparar a distância de direita e esquerda | |
| 154 | {
| |
| 155 | directionn = Lgo; //Vire à esquerda | |
| 156 | } | |
| 157 | else {
| |
| 158 | if(Lspeedd <= Rspeedd) //se a distância for menor ou igual à distância no lado direito | |
| 159 | {
| |
| 160 | directionn = Rgo; //Vire à direita | |
| 161 | } | |
| 162 | } | |
| 163 | } | |
| 164 | else | |
| 165 | {
| |
| 166 | directionn = Fgo; | |
| 167 | } | |
| 168 | myservo.write(90); | |
| 169 | delay(delay_time); | |
| 170 | } | |
| 171 | void ask_pin_F() // Medir a distância em frente | |
| 172 | {
| |
| 173 | digitalWrite(outputPin, LOW); //ultrasom 2us baixo nível | |
| 174 | delayMicroseconds(2); | |
| 175 | digitalWrite(outputPin, HIGH); // ultrasom de alta transmissão 10us, há pelo menos 10us | |
| 176 | delayMicroseconds(11); | |
| 177 | digitalWrite(outputPin, LOW); // ultrasom baixo nível | |
| 178 | float Fdistance = pulseIn(inputPin, HIGH); //medir o tempo | |
| 179 | Fdistance= Fdistance/5.8/10; // Tempo de distância （cm） | |
| 180 | Fspeedd = Fdistance; // | |
| 181 | } | |
| 182 | void ask_pin_L() // | |
| 183 | {
| |
| 184 | delay(delay_time); | |
| 185 | digitalWrite(outputPin, LOW); // | |
| 186 | delayMicroseconds(2); | |
| 187 | digitalWrite(outputPin, HIGH); // | |
| 188 | delayMicroseconds(11); | |
| 189 | digitalWrite(outputPin, LOW); // | |
| 190 | float Ldistance = pulseIn(inputPin, HIGH); // | |
| 191 | Ldistance= Ldistance/5.8/10; // | |
| 192 | Lspeedd = Ldistance; // | |
| 193 | } | |
| 194 | void ask_pin_R() // | |
| 195 | {
| |
| 196 | delay(delay_time); | |
| 197 | digitalWrite(outputPin, LOW); // | |
| 198 | delayMicroseconds(2); | |
| 199 | digitalWrite(outputPin, HIGH); // | |
| 200 | delayMicroseconds(11); | |
| 201 | digitalWrite(outputPin, LOW); // | |
| 202 | float Rdistance = pulseIn(inputPin, HIGH); // | |
| 203 | Rdistance= Rdistance/5.8/10; // | |
| 204 | Rspeedd = Rdistance; // | |
| 205 | } | |
| 206 | ||
| 207 | void loop() | |
| 208 | {
| |
| 209 | detection(); //Medir o ângulo e determinar o rumo a seguir | |
| 210 | if(directionn == 2) | |
| 211 | {
| |
| 212 | back(600); | |
| 213 | } | |
| 214 | if(directionn == 6) | |
| 215 | {
| |
| 216 | turnR(350); | |
| 217 | stopp(1); | |
| 218 | } | |
| 219 | if(directionn == 4) | |
| 220 | {
| |
| 221 | turnL(350); | |
| 222 | stopp(1); | |
| 223 | } | |
| 224 | if(directionn == 8) | |
| 225 | {
| |
| 226 | advance(10); | |
| 227 | ask_pin_F(); | |
| 228 | if(Fspeedd < 20) stopp(1); | |
| 229 | } | |
| 230 | } |
