/ Published in: Processing
Task 3 Code 2, ClauTIC League, by MadTeam
Expand |
Embed | Plain Text
Copy this code and paste it in your HTML
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * Tïtol: Tercer repte ClauTIC * * Objectiu: Seguir la lÃnia fins arribar a creta. * * Versió: 3.3.3 * * Robot: Tossut, Prototip A * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ // Definició dels pins dels motors int mesq_av = 12; // Motor esquerra per endavant al pin 12 int mesq_re = 11; // Motor esquerra per enradera al pin 11 int mesq_vel = 10; // Motor esquerra control velocitat al pin PWM 10 int mdret_av = 7; // Motor dret per endavant al pin 7 int mdret_re = 6; // Motor dret per enradera al pin 6 int mdret_vel = 5; // Motor dret control velocitat al pin PWM 5 //Definició dels pins del sensors int sen_esq = A0; int sen_dre = A1; int sen_cen = A2; //Definició de les variables que guardaran els valors int val_esq; int val_dre; int val_cen; int estat_color; //Definició del lÃmit entre colors int limit = 220; //més amunt negre, més avall blanc. // Definim les variables pel contador de temps unsigned long temps, tempsant=0, interval=50; void setup() { // Activem pins dels motors com sortida // Motor esquerra pinMode(mesq_vel, OUTPUT); pinMode(mesq_av, OUTPUT); pinMode(mesq_re, OUTPUT); // Motor dret pinMode(mdret_vel, OUTPUT); pinMode(mdret_av, OUTPUT); pinMode(mdret_re, OUTPUT); //Sensors de lÃnea com entrada pinMode(sen_esq, INPUT); pinMode(sen_dre, INPUT); pinMode(sen_cen, INPUT); Serial.begin(9600); } void loop() { val_esq = analogRead(sen_esq); val_dre = analogRead(sen_dre); val_cen = analogRead(sen_cen); /* if (segona==0){ Serial.print(val_esq); Serial.print(" "); Serial.print(val_cen); Serial.print(" "); Serial.print(val_dre); Serial.print(" - "); Serial.print(segona); Serial.println(" - "); }*/ estat_color = sequencia_color(limit, val_esq, val_cen, val_dre); /* if (segona==0){ Serial.print(" "); Serial.println(estat_color); //delay(200); }*/ switch (estat_color) { case 10: // blanc negre blanc 010 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 150, HIGH, LOW, 150); // motors endavant temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 1: // blanc blanc negre 001 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 100, HIGH, LOW, 0); // gira a la dreta ++ temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 100: // negre blanc blanc 100 temps = millis(); tempsant=millis(); while (temps < (tempsant+(interval))){ Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra -- temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 110: // negre negre blanc 110 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 11: // blanc negre negre 011 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 100, HIGH, LOW, 0); // gira a la dreta temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 111: // negre negre negre 111 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 101: // negre blanc negre 101 temps = millis(); tempsant=millis(); while (temps < (tempsant+interval)){ Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra temps=millis(); } Motors(LOW, LOW, 0, LOW, LOW, 0); break; case 0: // blanc blanc blanc 000 Motors(LOW, LOW, 0, LOW, LOW, 0); break; } } //FUNCIONS void Motors (char me_avan, char me_retr, int me_vel, char md_avan, char md_retr, int md_vel) { // Motor esquerra endavant o enrradera digitalWrite (mesq_av, me_avan); // Es passa al motor esquerra si anar endavant es HIGH o LOW digitalWrite (mesq_re, me_retr); // Es passa al motor esquerra si anar enradera es HIGH o LOW // Motor dret endavant o enrradera digitalWrite (mdret_av, md_avan); // Es passa al motor dret si anar endavant es HIGH o LOW digitalWrite (mdret_re, md_retr); // Es passa al motor dret si anar enradera es HIGH o LOW // Velocitat als motors analogWrite (mesq_vel, me_vel); // Es passa al motor esquerra la velocitat analogWrite (mdret_vel, md_vel); // Es passa al motor dret la velocitat } int sequencia_color(int valor_limit, int lec_esq, int lec_cen, int lec_dre) { int tira_color = 0; if (lec_esq > valor_limit) { tira_color = 100; // resultat de 1 * 100 }else { tira_color = 0; // resultat de 0 * 100 } if (lec_cen > valor_limit) { tira_color = tira_color + 10; // resultat de 1 * 10 } else { tira_color = tira_color + 0; // resultat de 0 * 10 } if (lec_dre > valor_limit) { tira_color = tira_color + 1; // el valor és 1 } else { tira_color = tira_color + 0; // el valor és 0 } return tira_color; //es retorna el valor de la variable tira_color }