/*----------------------------------------------------- Autore: ELETTRA ROBOTICS LAB Date: 2020-03-30 Programma Demo PoldinoTank PLUS Descrizione: Come la Demo02 - Evitaostacoli / seguilinea in abse al selettore (default) se all'avvio si tiene premuto PUSH fino al lampeggio del led si entra in modalità Bluetooth - controllo remoto tramite modulo Bluetooth HC_06 -----------------------------------------------------*/ //----------------------------------------------------- //DEFINIZIONE DEI PIN //----------------------------------------------------- #define S_prossimita_DX 13 //RA0 (sensore di prossimità DX) #define S_prossimita_SX 14 //RA1 (sensore di prossimità SX) //#define triggerPort 15 //RA2 (SRF_05 TRIGGER) //#define echoPort 16 //RA3 (SRF_05 ECHO) //#define USERLED 29 //RA4 (LED giallo della scheda Poldino_Board_Plus_Black) #define PIN_A5 17 //RA5 (disponibile) //#define I2C_SDA 0 //RB0 (I2C SDA della scheda Poldino_Board) //#define I2C_SCL 1 //RB1 (I2C SCL della scheda Poldino_Board_Plus_Black) #define PIN_B2 2 //RB2 (disponibile) #define PIN_B3 3 //RB3 (disponibile) #define A1_MOTA 4 //RB4 (Comando MotoreA1 della scheda Poldino_Board_Plus_Black) #define A2_MOTA 5 //RB5 (Comando MotoreA2 della scheda Poldino_Board_Plus_Black) #define B1_MOTB 6 //RB6 (Comando MotoreB1 della scheda Poldino_Board_Plus_Black) #define B2_MOTB 7 //RB7 (Comando MotoreB2 della scheda Poldino_Board_Plus_Black) #define Selettore 10 //RC0 (selettore per programma) //#define PWM_2 11 //RC1 (PWM2 della scheda Poldino_Board_Plus_Black) //#define PWM_1 12 //RC2 (PWM1 della scheda Poldino_Board_Plus_Black) //#define TX 8 //RC6 (Seriale TX della scheda Poldino_Board_Plus_Black)(Bluetooth) //#define RX 9 //RC7 (Seriale RX della scheda Poldino_Board_Plus_Black)(Bluetooth) #define PUSH 21 //RD0 (Pulsante AUX della scheda Poldino_Board) #define S_linea_SX 22 //RD1 (sensore di linea SX) #define S_linea_DX 23 //RD2 (sensore di linea DX) #define MYSERVO 24 //RD3 (servomotore da 5V) #define PIN_D4 25 //RD4 (disponibile) #define PIN_D5 26 //RD5 (disponibile) #define PIN_D6 27 //RD6 (disponibile) #define PIN_D7 28 //RD7 (disponibile) #define PIN_E0 18 //RE0 (disponibile) #define PIN_E1 19 //RE1 (disponibile) #define PIN_E2 20 //RE2 (disponibile) //--- #define BIANCO 1 #define NERO 0 //----------------------------------------------------- //DICHIARAZIONE VARIABILI //----------------------------------------------------- int sensdx=HIGH; int senssx=HIGH; int push_val=HIGH; const int triggerPort=15; const int echoPort=16; long durata; long distanza; long distanza_SX; long distanza_DX; u8 position=90; s8 dir=1; char dato; BOOL stato_nos=true; BOOL stato_bt=false; //----------------------------------------------------- //SETUP //----------------------------------------------------- void setup() { Serial.begin(9600); // I/O Settings pinMode(USERLED, OUTPUT); pinMode(A1_MOTA, OUTPUT); pinMode(A2_MOTA, OUTPUT); pinMode(B1_MOTB, OUTPUT); pinMode(B2_MOTB, OUTPUT); pinMode(triggerPort, OUTPUT); pinMode(echoPort, INPUT); pinMode(PUSH, INPUT); pinMode(S_linea_DX,INPUT); pinMode(S_linea_SX, INPUT); pinMode(S_prossimita_DX,INPUT); pinMode(S_prossimita_SX, INPUT); pinMode(Selettore, INPUT); digitalWrite(USERLED, LOW); m_fermo(); //configuro il servomotore servo.attach(MYSERVO); servo.setMinimumPulse(MYSERVO, 500); servo.setMaximumPulse(MYSERVO, 1800); //regolare posizione iniziale del servo agendo su questo numero servo.write(MYSERVO, 98); delay(10); //m_avanti(); PWM.setFrequency(3000); // 3KHz PWM.setDutyCycle(CCP1,650); //DX PWM.setDutyCycle(CCP2,650); //SX push_val = digitalRead(PUSH); if (push_val==0) stato_bt=true; }//end void setup //----------------------------------------------------- //LOOP //----------------------------------------------------- void loop() { if (stato_bt==true) { push_val=HIGH;//risetto la variabile alta per poterla rileggere delay(500); digitalWrite(USERLED, LOW); delay(500); digitalWrite(USERLED, HIGH); delay(500); digitalWrite(USERLED, LOW); delay(500); digitalWrite(USERLED, HIGH); delay(500); digitalWrite(USERLED, LOW); delay(2000); while (push_val==HIGH) {push_val = digitalRead(PUSH);} digitalWrite(USERLED, HIGH); while (true) { if (Serial.available()) { dato=Serial.read(); if(dato=='Z'){ digitalWrite(USERLED,LOW); } if(dato=='z'){ digitalWrite(USERLED,HIGH); } if(dato=='Y'){ digitalWrite(USERLED,LOW); m_avanti(); delay(100); digitalWrite(USERLED,HIGH); } if(dato=='y'){ digitalWrite(USERLED,LOW); m_fermo(); delay(100); digitalWrite(USERLED,HIGH); } if(dato=='W'){ digitalWrite(USERLED,LOW); m_indietro(); delay(100); digitalWrite(USERLED,HIGH); } if(dato=='w'){ digitalWrite(USERLED,LOW); m_NOS(); delay(100); digitalWrite(USERLED,HIGH); } if(dato=='V'){ digitalWrite(USERLED,LOW); m_ruota_dx() ; delay(100); digitalWrite(USERLED,HIGH); } if(dato=='U'){ digitalWrite(USERLED,LOW); m_ruota_sx(); delay(100); digitalWrite(USERLED,HIGH); } }//end if serial.available } //end while }//end if (stato_bt==true) else //se il pulsante non è premuto inizialmente eseguo l'evitaostacoli/seguilinea { //Test servo delay(500); servo.write(MYSERVO, 170); delay(500); servo.write(MYSERVO, 35); delay(500); servo.write(MYSERVO, 98); delay(500); while (push_val==HIGH) {push_val = digitalRead(PUSH);} digitalWrite(USERLED, HIGH); while (true) { // se il selettore è 0 allore SGUILINEA if (digitalRead(Selettore) ==0 ) { sensdx=digitalRead(S_linea_DX); senssx=digitalRead(S_linea_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(); } else //altrimenti EVITAOSTACOLI { delay(100); leggi_ultra(); if(distanza < 15){ digitalWrite(USERLED, HIGH); leggi_ultra(); if(distanza < 15){ m_fermo(); delay(100); m_indietro(); delay(500); m_fermo(); delay(100); servo.write(MYSERVO, 170); delay(500); leggi_ultra(); distanza_DX=distanza; servo.write(MYSERVO, 35); delay(500); leggi_ultra(); distanza_SX=distanza; if(distanza_SX < 35 && distanza_DX < 35){ m_ruota_sx(); delay(500); m_avanti(); } else { if(distanza_SX < distanza_DX){ servo.write(MYSERVO, 98); delay(500); m_ruota_dx(); delay(1000); m_avanti(); } else{ servo.write(MYSERVO, 98); delay(500); m_avanti(); } }//if else }//end if(distanza < 15) else{ digitalWrite(USERLED, LOW); m_avanti(); }//if else }// end if(distanza < 10) else{ digitalWrite(USERLED, LOW); m_avanti(); }//if else }//end else evitaostacoli }//end while (true) ciclo evitaostacoli/seguilinea }//end else //se il pulsante non è premuto inizialmente eseguo l'evitaostacoli/seguilinea }//end void loop //----------------------------------------------------- //SUBROUTINE //----------------------------------------------------- void leggi_ultra() { //porta bassa l'uscita del trigger digitalWrite( triggerPort, LOW ); delayMicroseconds( 2 ); //invia un impulso di 10microsec su trigger digitalWrite( triggerPort, HIGH ); delayMicroseconds( 10 ); digitalWrite( triggerPort, LOW ); durata = pulseIn( echoPort, HIGH, 10000 ); distanza = durata / 29 / 2; } void m_NOS() { toggle (stato_nos); if (stato_nos==false){ PWM.setDutyCycle(CCP1,1000); //DX PWM.setDutyCycle(CCP2,1000); //SX } else { PWM.setDutyCycle(CCP1,750); //DX PWM.setDutyCycle(CCP2,750); //SX } } void m_fermo() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, LOW); } void m_avanti() { digitalWrite(A1_MOTA, HIGH); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, HIGH); digitalWrite(B2_MOTB, LOW); } void m_indietro() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, HIGH); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, HIGH); } void m_ruota_sx() { digitalWrite(A1_MOTA, HIGH); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, HIGH); } void m_ruota_dx() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, HIGH); digitalWrite(B1_MOTB, HIGH); digitalWrite(B2_MOTB, LOW); } void m_gira_sx() { digitalWrite(A1_MOTA, HIGH); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, LOW); } void m_gira_dx() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, HIGH); digitalWrite(B2_MOTB, LOW); } void m_gira_sx_ind() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, LOW); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, HIGH); } void m_gira_dx_ind() { digitalWrite(A1_MOTA, LOW); digitalWrite(A2_MOTA, HIGH); digitalWrite(B1_MOTB, LOW); digitalWrite(B2_MOTB, LOW); } //----------------------------------------------------- //-----------------------------------------------------