/* Arlotto 2004 : pgm test robot chenille */

#include <pic.h>
#include <stdio.h>
#define __DEBUG  // à commenter pour mode autonome

#ifdef __DEBUG 
__CONFIG (0x3739);
#else
__CONFIG(0x3D39);
#endif
#include "biosdem.h"
#include "hardware.h"
#include "move.h"
#include "compass.h"
#include "sonar.h"
#include "serial.h"
#include "tempo.h"
bank1 int bearing ;
bank1 int distance ;
extern volatile unsigned int direction,vitesse ; // modifiable n'importe où dans le programme
extern volatile signed char angle,speed ;
extern volatile unsigned char move_state,turn_state;
void main (void)
   {
     LEDS_DIR_INIT;
     LEDS_ON;
     SWITCH_DIR_INIT;
     init_move();
	 init_serial();
	 init_compass();
     init_tempo0();
     DelayMs(250);   
     DelayMs(250);
     DelayMs(250);   
     DelayMs(250);
     LEDS_OFF; 
     serial_puts("Hello This is Achille v0.1 !!\r");
     PEIE=1;
     GIE = 1 ;
     switch (SW_VALUE)
        {
          case 0 : // avance tant que d >70 puis tourne
           
            for ( ; ; )
               {
        
            distance = read_distance();
            if( distance > 70)
            {
             angle=0;
             speed = 10;
             RightLed = 1 ;
	        }
             else
            {
            speed  =10 ;
            angle=90;
            RightLed=0;
            while( turn_state!=TURN_DONE);
            angle=0;
            DelayMs(200); // attend une it tmr0
            }                    
         
         }
        case 1 :
           for ( ; ; )
            {
            distance = read_distance();
            if( distance > 70)
            {
             angle=0;
             speed = 10;
             RightLed = 1 ;
	        }
             else
            {
             if ( distance < 20 )
               {
                 angle=0;
                 speed = -40 ;
                 while(read_distance()<70); 
               }
             else
              {
                speed  =10 ;
                angle=90;
                RightLed=0;
                while( turn_state!=TURN_DONE);
                angle=0;
               DelayMs(200); // attend une it tmr0
              }    
             }                    
            }
          case 2 :
           for ( ; ; )
            {
            distance = read_distance();
            if( distance > 70)
            {
             angle=0;
             speed = 10;
             RightLed = 1 ;
	        }
             else
            {
             if ( distance < 20 )
               {
                 angle=0;
                 speed = -40 ;
                 BackRightLed = 0 ;
                 start_tempo0(1500);
                 while((read_distance()<70)&&(!tempo0_done()));
                if ( tempo0_done() )
                 {
                   BackRightLed = 1 ;
                   speed  =10 ;
                    angle=-90;
                     RightLed=0;
                     while( turn_state!=TURN_DONE);
                     angle=0;
                     DelayMs(200); // attend une it tmr0
                 }
               }
             else
              {
                  speed  =10 ;
                          angle=90;
                          RightLed=0;
                          while( turn_state!=TURN_DONE);
                          angle=0;
                          DelayMs(200); // attend une it tmr0
              }    
             }                    
            }
      }
     
   }