/* PneumaticRobot1.nqc      Gennaio 2007
   Questo programma utilizza:
   Morore_A per ruotare il braccio del Robot
   Morore_B per azionare la pompa e l'interruttore pneumatico
     (Apertura e chiusura Mano)
   Morore_C per alzare e abbassare 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 50K
   Sensore_3 (rotazione) per controllare l'interruttore pneumatico
*/

// Definizioni delle costanti del programma
#define CENTER 0             // posizione centrale sensore di rotazione
#define ROTDX -45            // posizione destra sensore di rotazione
#define ROTSX 45             // posizione sinistra sensore di rotazione
#define ROTSWOPEN 250        // rotazione interruttore pneumatico apertura
#define ROTSWCLOSE -250      // rotazione interruttore pneumatico chiusura
// Definizioni delle variabili del programma
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);    
  SetSensor (SENSOR_3, SENSOR_ROTATION);         // sensore rotaz. int. pneum.

// ****************************  INIZIO PROGRAMMA  ****************************
  start angle_sensor;                 // Avvia il task per lettura rotazione
  open();                             // Vai alla SUB per aprire la mano
  ClearSensor (SENSOR_3);                        // Reset iniziale rotazione
  OnRev(OUT_C); Wait(200);                       // Braccio su per 2 sec.
  OnRev(OUT_C);                                  // Braccio su fino a ...
  until (SENSOR_1 == 1);                         // ... posizione braccio alto
  Off(OUT_C);                                    // Stop braccio su
  OnFwd(OUT_C); Wait(100);                       // Braccio giu per 1 sec.
  OnFwd(OUT_C);                                  // Braccio giu fino a ...
  until (SENSOR_1 == 1);                         // ... posizione braccio basso
  Off(OUT_C);                                    // Stop braccio giu
  close();                            // Vai alla SUB per chiudere la mano
  rotatedx();                         // Vai alla SUB per ruotare a destra
  open();                             // Vai alla SUB per aprire la mano
  rotatesx();                         // Vai alla SUB per ruotare a sinistra
  OnFwd(OUT_C);                                  // Braccio giu fino a NON ...
  until (SENSOR_1 == 0);                         // ... posizione braccio alto
  OnFwd(OUT_C); Wait(200);                       // Braccio giu per 2 sec.
  Off(OUT_C);                                    // Stop braccio giu
  PlaySound (SOUND_FAST_UP);                     // Suono di fine programma
  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 la documentazione)
    temp1 = ((27 * SENSOR_2)/(1023 - SENSOR_2) * 2) - 135 ;
    angolo = temp1;
    // Visualizza la posizione del sensore ad Angolo
    //SetUserDisplay(angolo, 0);
   }
}

// **********************  SUB PER CHIUDERE LA MANO  **************************
// Questa sub permette al Robot di chiudere la mano e prelevare il pezzo
sub close()
{
  OnRev(OUT_B);                                  // On motore pompa ...
  // Visualizza la posizione del sensore di rotazione
  SetUserDisplay(SENSOR_3, 0);
  until (SENSOR_3 < ROTSWCLOSE);                 // ... fino a mano chiusa
  Toggle(OUT_B);                                 // Inverti motore pompa ...
  until (SENSOR_3 >= 0);                         // ... fino a 0
  Off(OUT_B);                                    // Stop motore pompa
}

// ***********************  SUB PER APRIRE LA MANO  **************************
// Questa sub permette al Robot di aprire la mano e posizionare il pezzo
sub open()
{
  OnFwd(OUT_B);                                  // On motore pompa ...
  // Visualizza la posizione del sensore di rotazione
  SetUserDisplay(SENSOR_3, 0);
  until (SENSOR_3 > ROTSWOPEN);                  // ... fino a mano aperta
  Toggle(OUT_B);                                 // Inverti motore pompa ...
  until (SENSOR_3 <= 0);                         // ... fino a 0
  Off(OUT_B);                                    // Stop motore pompa
}

// *******************  SUB PER RUOTARE A DESTRA IL ROBOT  ********************
// Questa sub permette al Robot di ruotare a destra
sub rotatedx()
{
  SetUserDisplay(angolo, 0);  // Visualizza la posizione del sensore ad Angolo

  OnRev(OUT_C); Wait(400);                       // Braccio su per 4 sec.
  Off(OUT_C);                                    // Stop braccio su

  OnFwd(OUT_A);                                  // Braccio a Dx fino a ...
  until (angolo < ROTDX);                        // ... posizione destra
  PlayTone(247,40);                              // Suono posizione destra
  Off (OUT_A);                                   // Stop rotazione braccio

  OnFwd(OUT_C);                                  // Braccio giu fino a ...
  until (SENSOR_1 == 1);                         // ... posizione braccio basso
  Off(OUT_C);                                    // Stop braccio giu
}

// *********  SUB PER RUOTARE A SINISTRA (FINO AL CENTRO) IL ROBOT  ***********
// Questa sub permette al Robot di ruotare a sinistra (fino al centro)
sub rotatesx()
{
  SetUserDisplay(angolo, 0);  // Visualizza la posizione del sensore ad Angolo

  OnRev(OUT_C); Wait(400);                       // Braccio su per 4 sec.
  Off(OUT_C);                                    // Stop braccio su

  OnRev(OUT_A);                                  // Braccio a Sx fino a ...
  until (angolo > CENTER);                       // ... posizione centrale
  PlayTone(247,40);                              // Suono posizione centrale
  Off (OUT_A);                                   // Stop rotazione braccio

  OnRev(OUT_C);                                  // Braccio su fino a ...
  until (SENSOR_1 == 1);                         // ... posizione braccio alto
  Off(OUT_C);                                    // Stop braccio su
}