//serial protocol #define GIRO_H 'A' #define GIRO_AH 'H' #define FRENTE 'F' #define PARAR 'P' // Quantidade de pulsos necessários para os movimentos #define PULSOS_FRENTE 70 #define PULSOS_GIRO 35 //PINOS int encoder = 2; //Pino ligado ao pino D0 do sensor optico // Motor direito int pino_A_IA = 11; int pino_A_IB = 12; // Motor esquerdo int pino_B_IA = 5; int pino_B_IB = 6; int PWM = 200; // Modifica a intensidade do motor volatile byte pulsos; // Variável para contar pulsos char instruction; // Armazena o comando void setup() { // Abre a comunicação serial com BaudRate = 9600 Serial.begin(9600); //Pino do sensor optico como entrada pinMode(encoder, INPUT); // Vendo o prototipo de frente (mais distante do arduino) // é o lado direito. Configura Motor com o saida. pinMode( pino_A_IB, OUTPUT ); pinMode( pino_A_IA, OUTPUT ); // Vendo o prototipo de frente (mais distante do arduino) // é o lado esquerdo. Configura Motor com o saida. pinMode( pino_B_IB, OUTPUT ); pinMode( pino_B_IA, OUTPUT ); // Configura estado inicial = parado. digitalWrite( pino_B_IB, LOW ); digitalWrite( pino_B_IA, LOW ); digitalWrite( pino_A_IB, LOW ); digitalWrite( pino_A_IA, LOW ); pulsos = 0; delay(500); } //Incrementa pulsos à cada borda de descida void contador() { pulsos++; } void Pausa(){ detachInterrupt(0); digitalWrite( pino_B_IB, LOW ); digitalWrite( pino_B_IA, LOW ); digitalWrite( pino_A_IB, LOW ); digitalWrite( pino_A_IA, LOW ); delay(1000); pulsos = 0; attachInterrupt(0, contador, FALLING); } // Implementação do movimento para frente void MoverFrente(){ Pausa(); //Necessária para reiniciar contagem while(pulsos < PULSOS_FRENTE){ Serial.println(pulsos); digitalWrite( pino_A_IB, HIGH ); // direito = frente digitalWrite( pino_B_IB, LOW ); // esquerdo = frente analogWrite( pino_A_IA, 255-PWM ); // PWM = rapido analogWrite( pino_B_IA, 255-PWM ); // PWM = rapido } Pausa(); } // Implementação do giro 90o horário void Giro_Horario(){ Pausa(); //Necessária para reiniciar contagem while(pulsos < PULSOS_GIRO){ digitalWrite( pino_A_IB, HIGH ); // direito = frente digitalWrite( pino_B_IB, HIGH ); // esquerdo = reversa analogWrite( pino_A_IA, 255-PWM ); // PWM = rapido analogWrite( pino_B_IA, 255-PWM ); // PWM = rapido } Pausa(); } // Implementação do giro 90o anti-horário void Giro_AntiHorario(){ Pausa(); //Necessária para reiniciar contagem while(pulsos < PULSOS_GIRO){ digitalWrite( pino_A_IB, LOW ); // direito = reversa digitalWrite( pino_B_IB, LOW ); // esquerdo = frente analogWrite( pino_A_IA, 255-PWM ); // PWM = rapido analogWrite( pino_B_IA, 255-PWM ); // PWM = rapido } Pausa(); } // Comonicacao serial void comunicacao(){ if (Serial.available() > 0) { instruction = Serial.read(); switch (instruction) { case FRENTE: MoverFrente(); break; case GIRO_H: Giro_Horario(); break; case GIRO_AH: Giro_AntiHorario(); break; case PARAR: Pausa(); break; default: break; } Serial.write("OK"); } } void loop() { comunicacao(); }