#define NUM_LIGHTSENSORS 7
/* Definitions for sensor() */

#define WHITE 0
#define BLACK 1

#define FORWARD   0
#define RIGHT    -1
#define LEFT      1

#define DIST_RIGHT       5
#define DIST_LEFT        4
#define DIST_FRONT       2

#define HOLD            145
#define STORE           20
#define COLLECT         226
#define DUMP            100 

#define ENTER         15
#define UP              17
#define DOWN            18
#define SERVO0    0
#define SERVO1    1
#define SERVO2    2

#define SERVO0_RIGHT     4000
#define SERVO0_LEFT       150
#define SERVO0_CENTER    2000 

#define SERVO1_RIGHT     3800
#define SERVO1_LEFT         0
#define SERVO1_CENTER    1850

#define SERVO2_RIGHT     4000
#define SERVO2_LEFT       100
#define SERVO2_CENTER    2000


#define ANALOG          0
#define DIGITAL         1
#define OTHER           -1
#define KNOB            32

#define CENTER_TOUCH    10

#define LED_OUTPUT_PORT 7
#define LED_SLEEP_TIME .2
                        
#define LIGHTSENSOR0      19          /***|frnt |***/
#define LIGHTSENSOR1      20          /*    012    */
#define LIGHTSENSOR2      21          /*           */
#define LIGHTSENSOR3      22          /*           */
#define LIGHTSENSOR4      23          /*     3     */
#define LIGHTSENSOR5      24          /*           */
#define LIGHTSENSOR6      25          /*           */
                                      /*    456    */
                  	              /*************/
int LightSensors[NUM_LIGHTSENSORS];


int LightSensor_Thresholds[NUM_LIGHTSENSORS];
int Raw_Lights_Data[NUM_LIGHTSENSORS];
int LightSensors_White_ON[NUM_LIGHTSENSORS];
int LightSensors_Black_ON[NUM_LIGHTSENSORS];
int LightSensors_White_OFF[NUM_LIGHTSENSORS];
int LightSensors_Black_OFF[NUM_LIGHTSENSORS];
int Current_Light_Data[NUM_LIGHTSENSORS];
int Sensor_Colors[NUM_LIGHTSENSORS];

int Our_Side;
int Line_Color;
int Current_Color;

int mtrs[];



/***********************************************************/
void rotate_servos()
{
        servo(SERVO0, 3900);
        servo(SERVO1, 2700);
        servo(SERVO2, 1300);

        sleep(.5);        
}

/***********************************************************/
void set_servos(int direction)
{
        if (direction == FORWARD)
        {
                servo(SERVO0, SERVO0_CENTER);
                servo(SERVO1, SERVO1_CENTER);
                servo(SERVO2, SERVO2_CENTER);
        }
        else if (direction == LEFT)
        {
                servo(SERVO0, SERVO0_LEFT);
                servo(SERVO1, SERVO1_LEFT);
                servo(SERVO2, SERVO2_LEFT);
        }
        else if (direction == RIGHT)
        {
                servo(SERVO0, SERVO0_RIGHT);
                servo(SERVO1, SERVO1_RIGHT);
                servo(SERVO2, SERVO2_RIGHT);
        }
}
/***********************************************************/
void veer_right()
{                                                             
        int degrees0 = (((SERVO0_CENTER * 8)+(SERVO0_RIGHT * 2)) / 10);
        int degrees1 = (((SERVO1_CENTER * 8)+(SERVO1_RIGHT * 2)) / 10);
        int degrees2 = (((SERVO2_CENTER * 8)+(SERVO2_RIGHT * 2)) / 10);
	
        servo(SERVO0, degrees0);
        servo(SERVO1, degrees1);
        servo(SERVO2, degrees2);
       
}
/**********************************************************************/
void veer_left() 
{
        int degrees0 = (((SERVO0_CENTER * 8)+(SERVO0_LEFT * 2)) / 10);
        int degrees1 = (((SERVO1_CENTER * 8)+(SERVO1_LEFT * 2)) / 10);
        int degrees2 = (((SERVO2_CENTER * 8)+(SERVO2_LEFT * 2)) / 10);   

        servo(SERVO0, degrees0);
        servo(SERVO1, degrees1);
        servo(SERVO2, degrees2);

}
/**********************************************************************/
void hard_right() 
{
        int degrees0 = (((SERVO0_CENTER * 4)+ (SERVO0_RIGHT * 3)) / 7);
        int degrees1 = (((SERVO1_CENTER * 4)+ (SERVO1_RIGHT * 3)) / 7);
        int degrees2 = (((SERVO2_CENTER * 4)+ (SERVO2_RIGHT * 3)) / 7);

        servo(SERVO0, degrees0);
        servo(SERVO1, degrees1);
        servo(SERVO2, degrees2);

}
/**********************************************************************/
void hard_left() 
{
        int degrees0 = (((SERVO0_CENTER * 4)+ (SERVO0_LEFT * 3)) / 7);
        int degrees1 = (((SERVO1_CENTER * 4)+ (SERVO1_LEFT * 3)) / 7);
        int degrees2 = (((SERVO2_CENTER * 4)+ (SERVO2_LEFT * 3)) / 7);

        servo(SERVO0, degrees0);
        servo(SERVO1, degrees1);
        servo(SERVO2, degrees2);
       
}
/**************************************************************/
void drive(int power0, int power1, int power2)
{
        motor(0, power0);
        motor(1, power1);
        motor(2, power2);
}
/***********************************************************/
void get_colors()
{
        int i;
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                if (Current_Light_Data[i] > LightSensor_Thresholds[i])
                        Sensor_Colors[i] = WHITE;
                else
                        Sensor_Colors[i] = BLACK;
        }


}

int set_position(int position, int feedback_port, int slack, int mtr0, int mtr1, int mtr2, int pwr0, int pwr1, int pwr2)
{
        int i;
        int escape_code=1;
        int diff = 0;
        int direction = 0;
        diff = position - sensor(feedback_port, ANALOG);
        while ((abs(diff) > slack) && (escape_code))
                {                       
                        if (diff >= 0)
                                direction = -1;
                        else
                                direction = 1;
                        motor (mtr0, direction * pwr0);
                        motor (mtr1, direction * pwr1);
                        motor (mtr2, direction * pwr2);
                        diff = position - sensor(feedback_port, ANALOG);
/*                        if(stuck(feedback_port, ANALOG, 1000.0, 20))
                        {
                                beep();
                                ao();
                                smb("It's Stuck");
                                escape_code = 0;
                                break;
                        }
                        else
                        {
                                escape_code = 1;        
                        }
*/                }
        ao();
        return escape_code;
}


int sensor (int port, int type)

{
        int val = -1;
        if ((port > 31) || (port < 0))
                {
                        if (port == KNOB)
                                val = knob();
                 }
        else
                {
                        if (type == ANALOG)
                                val = analog(port);
                        else
                                val = digital(port);
                }
        return val;
}

/************************************************************/
void orient(int iter)
{
        int sensor0,sensor4, sensor6;
        
        
        int val2, val4, val6;

        rotate_servos();
        get_lights(iter,LED_SLEEP_TIME);
        val2 = Current_Light_Data[2];
        val4 = Current_Light_Data[4];
        val6 = Current_Light_Data[6];

        if (val2 > LightSensor_Thresholds[2])
                sensor0 = 1;
        else
                sensor0 = 0;
        if (val4 > LightSensor_Thresholds[4])
                sensor4 = 1;
        else
                sensor4 = 0;
        if (val6 > LightSensor_Thresholds[6])
                sensor6 = 1;
        else
                sensor6 = 0;


/*      printf("%d|%d|%d", sensor0, sensor4, sensor6);
      smb("");
*/     
     /* now, positive left, negative right */

        if ((sensor0 == WHITE) && (sensor4 == WHITE) && (sensor6 == BLACK))
		{
                        Our_Side == WHITE;
                        spin_robot(1,-1);
               
	     	}
        else if ((sensor0 == WHITE) && (sensor4 == BLACK) && (sensor6 == WHITE)) 
		{
                        Our_Side == WHITE;

	     	}
        else if ((sensor0 == BLACK) && (sensor4 == WHITE) && (sensor6 == WHITE)) 
		{
                        Our_Side == WHITE;
                        spin_robot(1, 1);

                }
        else if ((sensor0 == WHITE) && (sensor4 == WHITE) && (sensor6 == WHITE)) 
		{
                        Our_Side == WHITE;
                        spin_robot(2, -1);
	     	}
        else if ((sensor0 == WHITE) && (sensor4 == BLACK) && (sensor6 == BLACK)) 
		{
                        Our_Side == BLACK;
                        spin_robot(1, 1);

	     	}
        else if ((sensor0 == BLACK) && (sensor4 == BLACK) && (sensor6 == BLACK)) 
		{
                        Our_Side == BLACK; 
                        spin_robot(2, 1);

	     	}
        else if ((sensor0 == BLACK) && (sensor4 == BLACK) && (sensor6 == WHITE)) 
		{
                        Our_Side == BLACK;
                        spin_robot(1, -1);
	     	}
        else if ((sensor0 == BLACK) && (sensor4 == WHITE) && (sensor6 == BLACK)) 
		{
                        Our_Side ==  BLACK;
                }
	else
                smb("ERROR!");

}

/***********************************************************/
void get_lights(int iter, float del)
{
        int i,j;
        int LightSensors_ON[NUM_LIGHTSENSORS];
        int LightSensors_OFF[NUM_LIGHTSENSORS];

        set_digital_out(7);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_ON[i] = Raw_Lights_Data[i];

        }
 
        clear_digital_out(7);
 /*
        sleep(del);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_OFF[i] = Raw_Lights_Data[i];

        }
*/
        for(i = 0; i <NUM_LIGHTSENSORS; i++)
        {
                Current_Light_Data[i] = LightSensors_ON[i];
               /* abs(LightSensors_ON[i]
                  - LightSensors_OFF[i]); */
        }


}
/***********************************************************/
void read_light_sensors(int iter)
{
       int i,j;
       int val;
       int sum[NUM_LIGHTSENSORS];

       /* Initialize Sums */
       for(j = 0; j < NUM_LIGHTSENSORS; j++)
             sum[i] = 0;
            

       for(j = 0; j < iter; j++)
       {
             for(i = 0; i < NUM_LIGHTSENSORS; i++)
             {   
                   val = analog(LightSensors[i]);   
                   sum[i] = sum[i] + val;
             }
       }

       for(i = 0; i < NUM_LIGHTSENSORS; i++)
       {
                Raw_Lights_Data[i] = sum[i]/iter;
       }
}
/***********************************************************/
void light_calibration(int iter)
{

        int i;

        smb("White");
        set_digital_out(7);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_White_ON[i] = Raw_Lights_Data[i];

        }

/*        clear_digital_out(7);
        sleep(LED_SLEEP_TIME);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_White_OFF[i] = Raw_Lights_Data[i];

        }

  */
        smb("Black");

        set_digital_out(7);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_Black_ON[i] = Raw_Lights_Data[i];

        }

  /*    clear_digital_out(7);
        read_light_sensors(iter);
        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensors_Black_OFF[i] = Raw_Lights_Data[i];

        }
  */
        clear_digital_out(7);
        smb("LDone");

        for(i = 0; i < NUM_LIGHTSENSORS; i++)
        {
                LightSensor_Thresholds[i] = ((LightSensors_Black_ON[i] +
                        LightSensors_White_ON[i])/2);

        }
/*                    ((abs(LightSensors_Black_ON[i] - LightSensors_Black_OFF[i]))
                    + (abs(LightSensors_White_ON[i] - LightSensors_White_OFF[i])))/2; 
  */


}
/***********************************************************/
void print_array(int array[])
{
        int i;
        int size;
        size = _array_size(array);
        for(i = 0; i < size; i++)
        {
              printf("\n");
              printf("%d|", array[i]);
              smb("");
        }
}
/***********************************************************/

void smb(char msg_str[])

{

        if (msg_str != "")
                printf("\n%s", msg_str);

        while (! digital(ENTER)) {}
        while (digital(ENTER)) {}

/*        while (! stop_button())
		{}
	while (stop_button())
		{}
  */
}
/****************************************************/

void follow_the_wall(int distance_port, int ideal)
{	

      /*  if (abs(analog(distance_port) - ideal) < 10) 
                drive(100,100,100); 
        else */ if (analog(distance_port) < (ideal - 20)) {
                 hard_left();
        } else if (analog(distance_port) > (ideal + 20)) {
                hard_right();
        } else if (analog(distance_port) < (ideal - 10)) {
                veer_left();
        } else if (analog(distance_port) > (ideal + 10)) {
                veer_right();
        } 

  /*      int distance;
        int thres;
        thres = 10;
        distance = analog(distance_port);
        if (abs(analog(distance_port) - ideal) > thres)
                veer(ideal, distance);
    */
}

void half_turns(int num, int feedback_port, int mtr0, int mtr1, int mtr2, int pwr0, int pwr1, int pwr2){

/*  Forward on motors = decreasing feedback val  */

        int i;
        int start_val;
        int thold = 10;
        start_val = analog(feedback_port);
        motor(mtr0, pwr0);
        motor(mtr1, pwr1);
        motor(mtr2, pwr2);
        for (i=0; i<num; i++)
        {
                while (abs(start_val - analog(feedback_port)) < 3*thold)
                {}
                while (abs(start_val - analog(feedback_port)) >= thold)
                {}
                start_val = analog(feedback_port);
        }

        ao();

}
/*****************************************************/
void veer(int ideal, int distance, float direction)
{
        float percent_error;
        int pos0, pos1, pos2;
        float diff;
        /* standard is left -1.0, right 1.0 */

        if (direction == 1.0)
                diff = (float)(distance-ideal);
        else
                diff = (float)(ideal-distance);


 /*       diff = (float)(distance-ideal);*/
        percent_error = diff/((float)(ideal));
        percent_error == percent_error * direction;
  /*      if ((percent_error) > 1.0 || (percent_error) < -1.0)
        {
                if (percent_error > 0.0)
                    percent_error == 1.0;
                else
                    percent_error == -1.0;
        }
    */
        pos0 = SERVO0_CENTER + (int)(2000.0*percent_error);
        pos1 = SERVO1_CENTER + (int)(1850.0*percent_error);        
        pos2 = SERVO2_CENTER + (int)(2000.0*percent_error);

        servo(SERVO0, pos0);
        servo(SERVO1, pos1);
        servo(SERVO2, pos2);
 
}

int back_wall()
{
       /* 1- both pushed, 0- none pushed, 2- left pushed, 4- right pushed */
       /* back_right- digital(13)  back_left - digital(12) */

       int status = 0;

       if ( digital(13) == 1 && digital(12) == 1)
             status = 1;
       else if (digital(13) == 1 && digital(12) == 0)
             status = 4;           
       else if (digital(13) == 0 && digital(12) == 1)
             status = 2;           
       else if (digital(13) == 0 && digital(12) == 0)
             status = 0;           
       
       return status;
}
int hit_wall()
{
       /* 1- both pushed, 0- none pushed, 2- left pushed, 4- right pushed */
       /* front_right- digital(9)  front_left - digital(11) */

       int status = 0;

       if ( digital(9) == 1 && digital(11) == 1)
             status = 1;
       else if (digital(9) == 1 && digital(11) == 0)
             status = 4;           
       else if (digital(9) == 0 && digital(11) == 1)
             status = 2;           
       else if (digital(9) == 0 && digital(11) == 0)
             status = 0;           
       
       return status;

}
void spin_robot(int degrees, int direction)
{

        int speed = 100;
        rotate_servos();

        drive(speed * direction, -speed * direction, speed * direction);
        if (degrees == 1)
               sleep(.775);
        else if (degrees == 2)
               sleep(1.505);
        else
               sleep(.075);   

        ao();

}

void move_arm (int level)

{
        set_position(level, KNOB, 2, 4, 5, 5, 100, 100, 100);

}

void pinch_claw()
{
        set_position (58, 16, 2, 3, 3, 3, 100, 100, 100);
}

void open_claw()
{
        set_position (96, 16, 2, 3, 3, 3, 100, 100, 100);
}


int fill_buffer (int port)
{
        
}

int stuck(int port, int type, float threshold, int thold)
{

        float begin_time = 0.0;
        int rv;
        int port_last = 0;
        int port_current = 0;
        begin_time = seconds();
        while ((seconds() - begin_time < (threshold/1000.0)) && (abs(port_last - port_current) < thold))
        {
                port_last = port_current;
                port_current = sensor(port, type);
        }
        if ((seconds() - begin_time) > (threshold/1000.0))
        {
                rv=1;
        }
        else
        {
                rv=0;
        }
        return rv;
}

/***********************************************************/
void main(){
        int i;
        float direction;
        int ideal;

        int ideal_left_wall;
        int ideal_right_wall;
        int ideal_front_wall;
        int ideal_left_trough;
        int ideal_right_trough;
        int ideal_front_ball;

        int right_at_wall;
        int left_at_wall;

        int distance;
        int iter = 15;
        int current;
        int test;
        int thold = 2;
        int port;
        int lcv;
        LightSensors[0] = LIGHTSENSOR0;
        LightSensors[1] = LIGHTSENSOR1;
        LightSensors[2] = LIGHTSENSOR2;
        LightSensors[3] = LIGHTSENSOR3;
        LightSensors[4] = LIGHTSENSOR4;
        LightSensors[5] = LIGHTSENSOR5;
        LightSensors[6] = LIGHTSENSOR6;
        smb("Arm tests...");
        move_arm (COLLECT);
        move_arm (HOLD);
        open_claw();
        pinch_claw();
        move_arm (STORE);
  
        light_calibration(iter); 

       printf("Ideal Right W");
        smb("");
        ideal_right_wall = analog(DIST_RIGHT);
        printf("Ideal Left W");
        smb("");
        ideal_left_wall = analog(DIST_LEFT);

        printf("Ideal Left T");
        smb("");
        ideal_left_trough = analog(DIST_LEFT);
        printf("Ideal Right T");
        smb("");
        ideal_right_trough = analog(DIST_FRONT);



        smb("Ready?");
        start_machine(22); 

        enable_servos();
        
        orient(iter);  
        move_arm(COLLECT);
        open_claw();

        set_servos(FORWARD);
        sleep(.3);
        drive(100,100,100);
       

        port = DIST_LEFT;
        direction = 1.0;
        while ((analog(DIST_FRONT) < 100) && (hit_wall() == 0))
        {
                distance = analog(port);
                veer(ideal_left_wall, distance, direction);
        }
        ao();
        pinch_claw();
        move_arm(DUMP);
        spin_robot(2, 1);


        drive(-100,-100,-100);

        while (back_wall() != 1)
        {
                if (back_wall() == 2)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,-1);
                        ao();
                }
                else if (back_wall() == 4)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,1);
                        ao();
                 }
                 else if (back_wall() == 0)
                        drive(-100,-100,-100);

                 set_servos(FORWARD);
                 drive(-100,-100,-100);
        }                        

        set_servos(FORWARD);
        move_arm(COLLECT);
        open_claw();


        sleep(.3);
        drive(100,100,100);
       

        port = DIST_RIGHT;
        direction = -1.0;
        while ((analog(DIST_FRONT) < 100) && (hit_wall() == 0))
        {
                distance = analog(port);
                veer(ideal_right_wall, distance, direction);
        }
        ao();

        if (hit_wall() == 1)
        {
                drive(-100,-100,-100);
                sleep(1.0);
        }
        pinch_claw();
        move_arm(DUMP);


        spin_robot(1,1);


        drive(-100,-100,-100);

        while (back_wall() != 1)
        {
            
                if (back_wall() == 2)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,-1);
                        ao();
                }
                else if (back_wall() == 4)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,1);

                        ao();
                 }
                 else if (back_wall() == 0)
                        drive(-100,-100,-100);

                 set_servos(FORWARD);
                 drive(-100,-100,-100);
        }                        
        
        set_servos(FORWARD);
        move_arm(COLLECT);
        open_claw();

        sleep(.3);
        drive(100,100,100);
       

        port = DIST_RIGHT;
        direction = -1.0;
        while ((analog(DIST_FRONT) < 100) && (hit_wall() == 0))
        {
                distance = analog(port);
                veer(ideal_right_trough, distance, direction);
        }
        ao();

        if (hit_wall() == 1)
        {
                drive(-100,-100,-100);
                sleep(1.0);
        }
        pinch_claw();
        move_arm(DUMP);


        drive(-100,-100,-100);

        while (back_wall() != 1)
        {
                if (back_wall() == 2)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,-1);

                        ao();
                }
                else if (back_wall() == 4)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,1);
                        ao();
                 }
                 else if (back_wall() == 0)
                        drive(-100,-100,-100);

                 set_servos(FORWARD);
                 drive(-100,-100,-100);
        }                        
        
        drive(100,100,100);
        sleep(.1);
        ao();
        pinch_claw();
        move_arm(STORE);
        spin_robot(1,1);


        drive(-100,-100,-100);

        while (back_wall() != 1)
        {

                if (back_wall() == 2)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,-1);
                        ao();
                }
                else if (back_wall() == 4)
                {
                        ao();
                        drive(100,100,100);
                        sleep(.2);
                        spin_robot(0,1);
                        ao();
                 }
                 else if (back_wall() == 0)
                        drive(-100,-100,-100);

                 set_servos(FORWARD);
                 drive(-100,-100,-100);
        }                        
        ao();


        set_servos(FORWARD);
        sleep(.3);
        drive(100,100,100);
       

        port = DIST_LEFT;
        direction = 1.0;
        while ((analog(DIST_FRONT) < 100) && (hit_wall() == 0))
        {
                distance = analog(port);
                veer(ideal_left_wall, distance, direction);
        }
        ao();

        disable_servos();
        ao();
        smb("Done");

}




/*************************************************************/
   /*
        printf("Ideal Front W");
        smb("");
        ideal_front_wall = analog(DIST_FRONT);
        printf("Ideal Front B");
        smb("");
        ideal_front_ball = analog(DIST_FRONT);
    */

/*        print_array(LightSensor_Thresholds);*/
 /*       start_machine(22);  */
/*        orient(iter); */
/*
        set_servos(FORWARD);
        
        ideal = analog(DIST_LEFT);

        drive(100,100,100);
        while (analog(DIST_FRONT) < 50)
        {
        current = analog(DIST_LEFT);
        veer(ideal,current);     

       follow_the_wall(DIST_LEFT,ideal);
        }
*/
  /*
        printf("Right @ W");
        smb("");
        right_at_wall = analog(DIST_RIGHT);
        printf("Left @ W");
        smb("");
        left_at_wall = analog(DIST_LEFT);
  */

/*        thold=8;
        set_servos(FORWARD);
        sleep(.2);
        test = analog(26);
        printf("\nTest Val:%d", test);
        smb("");
        test=55;
        printf("%d",set_position(test, 26, thold, 0, 1, 2, 100, 100, 100));
        ao();
        smb("I'm in start pos.");
        rotate_servos();
        smb("I've rotated servos");
        sleep(.2);
*/
/*        while ((analog(26) - test-thold) > thold)
        {drive(-100, 100, -100);}

        printf("%d",set_position(115, 26, thold, 0, 1, 2, 100, -100, 100));
        printf("%d",set_position(test, 26, thold, 0, 1, 2, 100, -100, 100));
        while ((analog(26) - test) < 2*thold)
        {drive(-100, 100, -100);}


        printf("%d",set_position(115, 26, thold, 0, 1, 2, 100, -100, 100));
        printf("%d",set_position(test, 26, thold, 0, 1, 2, 100, -100, 100));

        while ((analog(26)- test) < 2*thold)
        {drive(-100, 100, -100);}

        printf("%d",set_position(115, 26, thold, 0, 1, 2, 100, -100, 100));
        printf("%d",set_position(test, 26, thold, 0, 1, 2, 100, -100, 100));
        ao();
        smb("");*/

/*
        while (! digital(15))
        {
                rotate_servos();
                set_position(test, 26, thold, 0, 1, 2, 100, -100, 100);
                half_turns(6, 26, 0, 1, 2, 100, -100, 100);
                set_position(test, 26, thold, 0, 1, 2, 100, -100, 100);
                set_servos(FORWARD);
                sleep(.5);
                set_position(test, 26, thold, 0, 1, 2, 100, 100, 100);
                half_turns(2, 26, 0, 1, 2, 100, 100, 100);
                set_position(test, 26, thold, 0, 1, 2, 100, 100, 100);

                sleep(.5);

        }


}
*/