/* BipedeRobot1.nqc Febbraio 2006 Questo programma utilizza: Morore_A per muovere le gambe del robot Morore_B per muovere la testa del robot Morore_C per muovere il corpo del robot Sensore_1 (contatto) per controllare l'arco-paraurti Sensore_2 (contatto) per controllare i sensori delle gambe Sensore_3 (rotazione) per controllare la rotazione del corpo */ // Definizioni delle costanti del programma #define ROTSX 40 // valore di rotazione Sx #define ROTDX -40 // valore di rotazione Dx #define GAMBE 100 // tempo di partenza gambe #define PICCOLO 40 // tempo di spostamento piccolo gambe #define FINETESTA 450 // tempo per testa in posizione centrale // Definizioni delle variabili del programma int controllo; // controllo passo avanti o indietro task main() { // ******************** Configurazione sensori ******************** SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH); // sensore contatto arco SetSensorMode(SENSOR_1, SENSOR_MODE_BOOL); SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH); // sensore contatto gambe SetSensorMode (SENSOR_2, SENSOR_MODE_BOOL); SetSensor (SENSOR_3, SENSOR_ROTATION); // sensore rotazione corpo // *********************** INIZIO PROGRAMMA *********************** controllo = 0; // Reset variabile controllo ClearSensor (SENSOR_3); // Reset iniziale rotazione PlaySound (SOUND_FAST_UP); // Suono AVVIO OnRev(OUT_B); Wait(FINETESTA/2); // Rotazione ... Toggle(OUT_B);Wait(FINETESTA); // ... iniziale ... OnRev(OUT_B); // ... testa OnRev(OUT_C); // Rotazione corpo a Dx ... until (SENSOR_3 == ROTDX); // ... fino a ROTDX Off(OUT_C); // Stop rotazione corpo a Dx OnRev(OUT_A);Wait(GAMBE/2);Off(OUT_A); // Gamba Sx avanti start verifica_arco; // Start controllo arco repeat (20) // Ripeti 20 volte il ciclo { if (controllo == 0) // se controllo = 0 ... { // ... vai alla sub ... passo_avanti(); // ... passo avanti } else // se controllo = 1 ... { // ... vai alla sub ... passo_indietro(); // ... passo indietro } Toggle(OUT_B); // Inversione rotazione testa start verifica_arco; // Start controllo arco } // fine 20 cicli stop verifica_arco; // Stop controllo arco OnFwd(OUT_C); // Rotazione corpo a Sx ... until (SENSOR_3 == -1); // ... fino a pos. verticale Off(OUT_C); // Stop rotazione corpo OnFwd(OUT_A); // Gamba Dx avanti ... Wait(GAMBE/2);Off(OUT_A); // ... fino a pos. centrale PlaySound (SOUND_DOWN); // Suono FINE StopAllTasks (); // FINE PROGRAMMA } // ***************************** FINE PROGRAMMA ***************************** // ********************** TASK VERIFICA ARCO PARAURTI ************************ /* Questo task controlla se viene premuto il sensore di contatto presente sull'arco. Se premuto il robot emette un LOW_BEEP e la variabile controllo viene settata a 1; in questo modo nel task "main" verrą eseguita la sub "passo_indietro" */ task verifica_arco() { while(true) // ripeti sempre { if(SENSOR_1 == 1) // se sensore arco premuto ... { controllo = 1; // set variabile controllo a 1 PlaySound (SOUND_LOW_BEEP); // suono LOW_BEEP stop verifica_arco; // stop del task verifica arco } } } // **************************** SUB PASSO AVANTI ***************************** /* Questa sub fa eseguire al robot un passo completo in avanti. E' la sub che viene eseguita di default e viene ripetuta se non interviene il sensore dell'arco paraurti. */ sub passo_avanti() { PlaySound (SOUND_FAST_UP); // Suono passo avanti OnFwd(OUT_C); // Rotazione corpo a Sx ... until (SENSOR_3 == ROTSX); // ... fino a ROTSX Off(OUT_C); // Stop rotazione corpo a Sx OnFwd(OUT_A);Wait(PICCOLO); // Gamba Dx avanti per poco OnFwd(OUT_A); // Gamba Dx avanti ... until (SENSOR_2 == 1); // ... fino a sensore gambe Off(OUT_A); // Stop gamba Dx avanti OnRev(OUT_C); // Rotazione corpo a Dx ... until (SENSOR_3 == ROTDX); // ... fino a ROTDX Off(OUT_C); // Stop rotazione corpo a Dx OnRev(OUT_A);Wait(PICCOLO); // Gamba Sx avanti per poco OnRev(OUT_A); // Gamba Sx avanti ... until (SENSOR_2 == 1); // ... fino a sensore gambe Off(OUT_A); // Stop gamba Sx avanti } // *************************** SUB PASSO INDIETRO ***************************** /* Questa sub fa eseguire al robot un passo completo in dietro. E' la sub che viene eseguita una volta dopo che interviene il sensore dell'arco paraurti. */ sub passo_indietro() { PlaySound (SOUND_DOUBLE_BEEP); // Suono passo indietro OnFwd(OUT_A);Wait(PICCOLO); // Gamba Sx indietro per poco OnFwd(OUT_A); // Gamba Sx indietro ... until (SENSOR_2 == 1); // ... fino a sensore gambe Off(OUT_A); // Stop gamba Sx indietro OnFwd(OUT_C); // Rotazione corpo a Sx ... until (SENSOR_3 == ROTSX); // ... fino a ROTSX Off(OUT_C); // Stop rotazione corpo a Sx OnRev(OUT_A);Wait(PICCOLO); // Gamba Dx indietro per poco OnRev(OUT_A); // Gamba Dx indietro ... until (SENSOR_2 == 1); // ... fino a sensore gambe Off(OUT_A); // Stop gamba Dx indietro OnRev(OUT_C); // Rotazione corpo a Dx ... until (SENSOR_3 == ROTDX); // ... fino a ROTDX Off(OUT_C); // Stop rotazione corpo a Dx controllo = 0; // reset variabile controllo }