/* ArtRobot_AngleSensor.nqc Novembre 2006 Questo programma utilizza: Morore_A per alzare/abbassare il braccio del robot e aprire/chiudere la mano del Robot Morore_B non utilizzato Morore_C per ruotare il braccio del Robot Sensore_1 (contatto) per controllare il braccio (2 sensori in parallelo) Sensore_2 (Angle Sensor My Creation) per controllare la rotazione del braccio Angle Sensor 100K (vedi file .DOC) Sensore_3 (ottico) per controllare l'oggetto da prendere */ // Definizioni delle costanti del programma #define LIGHT_TO_DARK 3 // correzione per lettura pezzo nero #define CENTER 0 // posizione centrale sensore di rotazione // Definizioni delle variabili del programma int light; // lettura della luce ambiente int light_dark; // valore luce per lettura pezzo nero int sx_dx; // 0 = pezzo DARK trovato con rotazione a Sx // 1 = pezzo DARK trovato con rotazione a Dx int angolo; // Angolo calcolato partendo dal valore RAW letto int temp1; // Variabile temporanea per calcolo angolo task main() { // ************************* CONFIGURAZIONE SENSORI ************************* SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH); // sensori contatto braccio SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL); SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH); // sensore angolo rotazione SetSensorMode (SENSOR_2, SENSOR_MODE_RAW); SetSensorType(SENSOR_3, SENSOR_TYPE_LIGHT); // sensore ottico SetSensorMode (SENSOR_3, SENSOR_MODE_PERCENT); // **************************** INIZIO PROGRAMMA **************************** OnRev(OUT_A); // Braccio su fino a ... until (SENSOR_1 == 1); // ... posizione braccio alto Off(OUT_A); // Stop braccio su OnFwd(OUT_A); Wait(380); // Braccio giu per (3,8 sec.) Off(OUT_A); // Stop braccio giu // INIZIALIZZAZIONE VALORI LUCE AMBIENTE E LUCE DARK light = SENSOR_3; // Lettura luce ambiente e ... SetUserDisplay(light, 0); Wait(50); // ... visualizz. sul DISPLAY light_dark = (light - LIGHT_TO_DARK); // Set valore luce DARK SetUserDisplay(light_dark, 0); Wait(50); // INIZIO CICLO PER RICERCA PEZZO DARK start angle_sensor; // Avvia il task per ... // ... lettura rotazione start find_dark; // Avvia il task di ricerca /* Inizia una rotazione a Sx. Se non viene trovato un pezzo DARK, il robot esegue una rotazione a Dx e continua per 3 Volte. Se viene trovato un pezzo DARK, il robot emette un LOW_BEEP dopo di che si passa alla SUB "take_dark" */ while (true) // Ripeti fino a quando ... { // ... viene trovato ... if(SENSOR_3 < light_dark) // ... un pezzo DARK { Off (OUT_C); // Stop rotazione braccio PlaySound (SOUND_LOW_BEEP); // Suono LOW_BEEP light_dark = 0; // Disabilita luce DARK stop find_dark; // Stop al task di ricerca take_dark(); // ... vai alla sub per ... } // ... prendere il pezzo } StopAllTasks (); // ... FINE PROGRAMMA } // ***************************** FINE PROGRAMMA ***************************** // ****** TASK Lettura rotazione braccio (My creation Angle Sensor) ********* task angle_sensor() { while(true) { // Calcolo della posizione del sensore di rotazione // (N.B. per la formula di calcolo vedi il file .DOC) temp1 = ((27 * SENSOR_2)/(1023 - SENSOR_2) * 1) - 135 ; angolo = temp1; // Visualizza la posizione del sensore di rotazione SetUserDisplay(angolo, 0); } } // *********************** TASK Ricerca Pezzo DARK ************************** task find_dark() { repeat(3) // Ripeti 3 volte il ciclo { sx_dx = 0; // Rotazione Sx OnRev(OUT_C); Wait(500); // Braccio a Sx per 5 sec. sx_dx = 1; // Rotazione Dx OnFwd(OUT_C); // Braccio a Dx ... until (angolo < CENTER); // ... fino a pos. iniziale Off (OUT_C); // Stop rotazione braccio Wait(100); // Pausa 1 sec. } // Ripeti il ciclo light_dark = 0; // Disabilita luce DARK // Se dopo 3 cicli non viene trovato il pezzo DARK il robot ... // ... si riporta alla posizione iniziale OnRev(OUT_A); // Braccio su fino a ... until (SENSOR_1 == 1); // ... posizione braccio alto OnFwd(OUT_A); Wait(100); // Braccio giu per 1 sec. Off(OUT_A); // Stop braccio giu PlaySound (SOUND_LOW_BEEP); // Suono di fine programma PlaySound (SOUND_LOW_BEEP); // Suono di fine programma StopAllTasks (); // FINE PROGRAMMA } // ******************* SUB PER PRENDERE IL PEZZO DARK ************************ /* Questa sub permette al Robot di prendere il pezzo DARK e riposizionarlo alla sua Dx */ sub take_dark() { if(sx_dx == 0) // Se rotazione = Sx { OnFwd(OUT_C); Wait(200); // Braccio a Dx per 2 sec. Off (OUT_C); // Stop rotazione braccio OnFwd(OUT_A); // Braccio giu fino a ... until (SENSOR_1 == 1); // ... posizione mano aperta Off(OUT_A); // Stop braccio giu OnRev(OUT_C); Wait(250); // Braccio a Sx per 2,5 sec. Off (OUT_C); // Stop rotazione braccio } if(sx_dx == 1) // Se rotazione = Dx { OnRev(OUT_C); Wait(200); // Braccio a Sx per 2 sec. Off (OUT_C); // Stop rotazione braccio OnFwd(OUT_A); // Braccio giu fino a ... until (SENSOR_1 == 1); // ... posizione mano aperta Off(OUT_A); // Stop braccio giu OnFwd(OUT_C); Wait(250); // Braccio a Dx per 2,5 sec. Off (OUT_C); // Stop rotazione braccio } OnRev(OUT_A); Wait (200); // Braccio su per 2 sec. OnRev(OUT_A); // Braccio su fino a ... until (SENSOR_1 == 1); // ... posizione mano chiusa Off(OUT_A); // Stop braccio su OnFwd(OUT_C); // Braccio a Dx fino a ... until (angolo < CENTER); // ... posizione centrale PlayTone(247,40); // Suono posizione centrale OnFwd(OUT_C); Wait(400); // Braccio a Dx per 4 sec. Off (OUT_C); // Stop rotazione braccio OnFwd(OUT_A); Wait(100); // Braccio giu per 1 sec. OnFwd(OUT_A); // Braccio giu fino a ... until (SENSOR_1 == 1); // ... posizione mano aperta Off(OUT_A); // Stop braccio giu PlaySound(SOUND_CLICK); // Suono SOUND_CLICK Wait(50); // Pausa 0,5 sec. OnRev(OUT_C); Wait(200); // Braccio a Sx per 3 sec. Off (OUT_C); // Stop rotazione braccio OnRev(OUT_A); Wait(400); // Braccio su per 4 sec. Off(OUT_A); // Stop braccio su OnRev(OUT_C); // Braccio a Sx fino a ... until (angolo > CENTER); // ... posizione centrale PlayTone(247,40); // Suono posizione centrale Off (OUT_C); // Stop rotazione braccio OnRev(OUT_A); // Braccio su fino a ... until (SENSOR_1 == 1); // ... posizione braccio alto OnFwd(OUT_A); Wait(100); // Braccio giu per 1 sec. Off(OUT_A); // Stop braccio giu PlaySound (SOUND_FAST_UP); // Suono di fine programma StopAllTasks (); // FINE PROGRAMMA }