/* tavolo.c 
 *
 * Semplice robot autonomo.
 *
 * Esplora un tavolo piano finche' non incontra un bordo.
 * Arretra di un po', cambia direzione e poi riprende da capo.
 *
 * Utilizza il differential drive.
 * A: Motore Destro
 * B: vuoto
 * C: Motore Sinistro
 * 1: Sensore Touch
 * 2: vuoto
 * 3: vuoto
 */


/* per l'utilizzo dell'emulatore */
#ifdef EMULEGOS
#  include "emuLegOs.h"
#else
#  include <conio.h>
#  include <unistd.h>
#  include <dsensor.h>
#  include <dmotor.h>
#endif

#ifdef EMULEGOS
#  define RCXMAIN rcx_main
#else
#  define RCXMAIN main
#endif


#define TRUE 1
#define BRAKE_TIME 500
#define TURN_TIME 1500
#define CRUISE_SPEED 100

#define DX_SPEED(X) motor_a_speed(X)
#define DX_DIR(X) motor_a_dir(X)
#define SX_SPEED(X) motor_c_speed(X)
#define SX_DIR(X) motor_c_dir(X)

wakeup_t sensor_press_wakeup(wakeup_t data);
wakeup_t sensor_release_wakeup(wakeup_t data);

void vai_diritto();
void ferma(int tempo);
void vai_indietro();
void gira_a_destra(int tempo);

int RCXMAIN(int argc, char **argv) {

  while(TRUE){
    vai_diritto(); // Avanti ...
    wait_event(&sensor_press_wakeup,0); // ... finche' non trovi un bordo ...
    ferma(BRAKE_TIME); // ... e fermati!
    vai_indietro(); // Torna indietro ... 
    wait_event(&sensor_release_wakeup,0); // ... finche' non sei lontano dal bordo ...
    vai_indietro(); // ... e poi ancora per un po'.
    sleep(1);
    gira_a_destra(TURN_TIME); // Cambia direzione.
  }
  return 0;
}


wakeup_t sensor_press_wakeup(wakeup_t data) {
	return TOUCH_1 == 1;
}

wakeup_t sensor_release_wakeup(wakeup_t data) {
	return TOUCH_1 == 0;
}


void vai_diritto() {
  DX_SPEED(CRUISE_SPEED); 
  SX_SPEED(CRUISE_SPEED);
  DX_DIR(fwd);
  SX_DIR(fwd);
};

void ferma(int tempo) {
  DX_DIR(brake);   
  SX_DIR(brake);
  msleep(tempo);
  DX_DIR(off);   
  SX_DIR(off);
};

void vai_indietro() {
  DX_SPEED(CRUISE_SPEED); 
  SX_SPEED(CRUISE_SPEED);
  DX_DIR(rev);
  SX_DIR(rev);
};

void gira_a_destra(int tempo) {
      DX_SPEED(CRUISE_SPEED);   
      SX_SPEED(CRUISE_SPEED);
      DX_DIR(rev);  
      SX_DIR(fwd);
      msleep(tempo);
};