/*----------------------------------------------------- Author: ELETTRA ROBOTICS LAB Date: 15/12/2020 Description:Elettrino Demo02 Program seguilinea -----------------------------------------------------*/ //----------------------------------------------------- //DEFINIZIONE DEI PIN //----------------------------------------------------- #define SPEED1 6 // pwm #define SPEED2 5 // pwm #define PB6 10 // mot #define PB7 11 // mot #define PB4 8 // mot #define PB5 9 // mot #define S_SX 2 // sensore SX #define S_DX 3 //sensore DX //#define SEL 2 // selettore #define LIN_SX 4 // sensoreLine SX #define LIN_DX 12 // sensore Linea DX #define PUSH 7 // PULSANTE #define USERLED 13 #define BIANCO 1 #define NERO 0 //----------------------------------------------------- //DICHIARAZIONE VARIABILI //----------------------------------------------------- int senssx, sensdx, push_val=HIGH; //----------------------------------------------------- //SETUP //----------------------------------------------------- void setup() { // I/O Settings pinMode(USERLED, OUTPUT); pinMode(SPEED1, OUTPUT); pinMode(SPEED2, OUTPUT); pinMode(PB6, OUTPUT); pinMode(PB7, OUTPUT); pinMode(PB4, OUTPUT); pinMode(PB5, OUTPUT); pinMode(S_DX,INPUT); pinMode(S_SX, INPUT); pinMode(LIN_DX,INPUT); pinMode(LIN_SX, INPUT); pinMode(PUSH, INPUT); digitalWrite(USERLED, HIGH); m_fermo(); analogWrite(SPEED1,160); analogWrite(SPEED2,160); } //----------------------------------------------------- //LOOP //----------------------------------------------------- void loop() { while (push_val==HIGH) {push_val = digitalRead(PUSH);} digitalWrite(USERLED, LOW); while (true) { // SEGUILINEA sensdx=digitalRead(LIN_DX); senssx=digitalRead(LIN_SX); if((sensdx==BIANCO && senssx==BIANCO)||(sensdx==NERO && senssx==NERO)) m_avanti(); if(sensdx==NERO && senssx==BIANCO) m_gira_dx(); if(sensdx==BIANCO && senssx==NERO) m_gira_sx(); } //ciclo }//main //----------------------------------------------------- //FUNZIONI //----------------------------------------------------- void m_fermo() { digitalWrite(PB4, LOW); digitalWrite(PB5, LOW); digitalWrite(PB6, LOW); digitalWrite(PB7, LOW); } void m_avanti() { digitalWrite(PB4, HIGH); digitalWrite(PB5, LOW); digitalWrite(PB6, HIGH); digitalWrite(PB7, LOW); } void m_indietro() { digitalWrite(PB4, LOW); digitalWrite(PB5, HIGH); digitalWrite(PB6, LOW); digitalWrite(PB7, HIGH); } void m_ruota_sx() { digitalWrite(PB4, HIGH); digitalWrite(PB5, LOW); digitalWrite(PB6, LOW); digitalWrite(PB7, HIGH); } void m_ruota_dx() { digitalWrite(PB4, LOW); digitalWrite(PB5, HIGH); digitalWrite(PB6, HIGH); digitalWrite(PB7, LOW); } void m_gira_sx() { digitalWrite(PB4, HIGH); digitalWrite(PB5, LOW); digitalWrite(PB6, LOW); digitalWrite(PB7, LOW); } void m_gira_dx() { digitalWrite(PB4, LOW); digitalWrite(PB5, LOW); digitalWrite(PB6, HIGH); digitalWrite(PB7, LOW); } void m_gira_sx_ind() { digitalWrite(PB4, LOW); digitalWrite(PB5, LOW); digitalWrite(PB6, LOW); digitalWrite(PB7, HIGH); } void m_gira_dx_ind() { digitalWrite(PB4, LOW); digitalWrite(PB5, HIGH); digitalWrite(PB6, LOW); digitalWrite(PB7, LOW); }