Project 9

#include <atx.h>
#define BLACK 200
#define LEFT_SENSOR 5
#define RIGHT_SENSOR 4
int state = 1 ;
void setup()
{
  lcd("pressSW1");
  sw1_press();
  beep();
}
void loop()
 {
 go_forward(100);
 delay(400);
 spin_right(100);
 delay(470);
 go_forward(100);
 delay(370);
 motor_stop(ALL);
 delay(200);
 go_forward(100);
 delay(800);
 motor_stop(ALL);
 delay(200);
 follow_to_intersection();
 motor_stop(ALL);
 delay(200);
 spin_left(100);
 delay(330);
 timed_follow2(16000);
 motor_stop(ALL);
 delay(200);
 go_forward(100);
 delay(400);
 motor_stop(ALL);
 delay(200);
 timed_follow2(9000);
 go_forward(100);
 delay(300);
 motor_stop(ALL);
 sw1_press();
 }

void spin_left(int t_power)
{
  motor(0,t_power);
  motor(1,t_power);
  motor(2,-t_power);
  motor(3,-t_power);
}
void turn_left(int t_power)
{
  motor(0,t_power);
  motor(1,t_power);
  motor(2,0);
  motor(3,0);
}
void spin_right(int t_power)
{
  motor(0,-t_power);
  motor(1,-t_power);
  motor(2,t_power);
  motor(3,t_power);
}
void go_forward(int f_power)
{
  motor(0,f_power);
  motor(1,f_power);
  motor(2,f_power);
  motor(3,f_power);
}

void timed_follow( long ms )
{
  long int start = millis(); // page 23
  while( millis() - start < ms )
  {
    int left = analogRead(LEFT_SENSOR);
    int right = analogRead(RIGHT_SENSOR);
    if(left < 200 && right < 200)
    {
      spin_left(100);
      delay(300);
    }
    else if( left < 200 )
    {
      spin_left(50);
    }
    else if( right < 200 )
    {
      spin_right(50);
    }
    else
    {
      go_forward(20);
    }
  }
}
void timed_follow2( long ms )
{
  long int start = millis(); // page 23
  while( millis() - start < ms )
  {
    int left = analogRead(LEFT_SENSOR);
    int right = analogRead(RIGHT_SENSOR);
    if( left < 200 )
    {
      spin_left(70);
    }
    else if( right < 200 )
    {
      spin_right(70);
    }
    else
    {
      go_forward(20);
    }
  }
}

void follow_to_intersection()
{
  int left = analogRead(5);
  int right = analogRead(4);

  do {
    int left = analogRead(5);
    int right = analogRead(4);
    if( left < 150 && right < 150 )
    {
      break;
    }
    else if( left < 200 )
    {
      spin_left(50);
    }
    else if( right < 200 )
    {
      spin_right(50);
    }
    else
    {
      go_forward(20);
    }
  }
  while( 1 );
}