MarcFriedenberg.com

#define LEFT 1 /*the left motor is on analog1*/
#define RIGHT 0 /*the right motor is on analog0*/
#define WSTRAIGHT 1865 /*the position to turn the wheel towards to go in a straight line*/
#define SSTRAIGHT 2000 /*the position to turn the sensor/fan towards to point in a straight line*/
#define FPOWER -55 /*forward power*/
#define IRSENSOR analog(3) /*the IR sensor is on analog2*/
#define LRANGE GP2D02_13 /*left range finder on analog 13*/
#define FRANGE GP2D02_14 /*front range finder on analog 14*/
#define LINESENSE analog(3) /*the line sensor is on analog 4*/
#define SENSERVO servo1 /*the IR sensor is mounted on servo 1*/
#define WSERVO servo0 /*the wheel is on servo 0*/
#define LINEFOUND 160 /*the value to check for the white line*/

/*SHAFT - Completed March 22, 2001
Design Team -
Computer engineer: Marc Friedenberg
Mechanical engineer: Dan McAdams
Electrical engineer: Brian Luff
Documentarian: Jenn Kirkpatrick

Purpose -
        Finds and mercilessly destroys candles placed in room 4
        of the Trinity Robotics Competition maze. 

Directions -
        Place the cart so it is facing the wall parallel to the door of room
        4. Turn on the handyboard, and place the cart down so it is 127
        units from the wall. Press the start button twice. Voila!

Areas for improvement -
        1) Go home after extinguising candle
        2) Move straight towards candle rather than using one or two arcs
        3) Use course-correction to stay parallel to the walls
        4) Allow the candle to be placed in any room
        5) Provide support to move up and down ramps
        6) Make a function to support making a turn of a specified radius
        7) Check that the candle has been blown out*/        

void main()
{
    init_expbd_servos(1); /*activate the servos*/
    GP2D02_enable(1); /*enable the range finders*/
    SENSERVO=SSTRAIGHT; /*set the sensor/fan straight*/
    WSERVO=WSTRAIGHT; /*set the front wheel straight*/

    while(!start_button()) /*before the cart has turned on...*/
      {
        printf("%d\n",LRANGE); /* display the left range value, preferably 127*/
        sleep(.2);
    }
    while (start_button()) {} /*do nothing while start is being held...*/

    sleep(.5); /*wait half a second before moving the cart*/
    motor(RIGHT,FPOWER); /*turn on the motors*/
    motor(LEFT,FPOWER);

    while (FRANGE<=73) /*the cart is moving straight until the front range is greater than 73...*/
      {
        printf("%d\n",FRANGE); /*display the front range value*/
        sleep(.2);
    }

    left_turn(); /*begin the left turn*/
} /*this is where the entire program ends*/

void left_turn() /*makes a left turn into room 4*/
{
    WSERVO=1000; /*turns the wheel servo*/
    sleep (.45); /*travels at this angle for .45 seconds*/
    WSERVO=1450; /*makes the angle a little bit more obtuse*/

    motor(RIGHT,-40); /*change the power to the motors*/
    motor(LEFT,-40);

    while (1) /*the cart will continue to move at this angle until the line is found*/
      {
        if (LINESENSE<=LINEFOUND) /*if the line is found...*/
          {
            beep(); /*beep twice (useful for debugging purposes)*/
            beep();
            motor_kill(); /*stop the cart*/
            ir_sweep(); /*find the candle, blow it out (calls several other functions)*/
            break; /*after the candle is blown out, exit the while loop, return to main, and end program*/
        }
    }
}

void motor_kill() /*used to stop the cart*/
{
    motor(RIGHT,10); /*go in reverse for 1 second*/
    motor(LEFT,10);
    sleep (1.);
    motor(RIGHT,0); /*stop both motors*/
    motor(LEFT,0);
}

void ir_sweep() /*moves the cart to the white piece of paper surrounding the candle*/
{
    scan(); /*points the fan and wheel towards the candle*/
    if (LINESENSE>=LINEFOUND) /*if we're not on white...*/
      {
        motor (LEFT,FPOWER); /*turn on both motors*/
        motor (RIGHT,FPOWER);
        while (1) /*keep looping this...*/
          {

            if (LINESENSE<=LINEFOUND) /*if we cross white stuff...*/
              {
                motor_kill(); /*stop the cart*/
                scan(); /*points the fan and wheel towards the candle*/
                blow_out_candle(); /*blow out the candle*/
                break; /*exit the while loop, return to main, exit program*/
            }

        }
    }

    else
      {
        blow_out_candle(); /*blow out the candle*/
    }

}

void blow_out_candle() /*blows out the candle*/
{
    motor(2,100); /*turn on the fan*/
    sleep(2.); /*keep it on for two seconds*/
    motor(2,0); /*turn off the fan, return to ir_sweep()*/
}

void scan() /*points the fan and wheel towards the candle*/
{

    int angle,lposition,lreading=200; /*declare variables used for scanning routine*/
    for (angle=4100;angle>=0;angle-=75) /*decrement the sensor/fan servo from 4100 to 0*/
      {
        SENSERVO=angle; /*turn the servo to the specified angle*/
        sleep(.1); /*wait .1 seconds to allow the servo to turn*/
        if (IRSENSOR<=lreading) /*if the current reading of the sensor is lower than lowest recorded value..*/
          {
            lreading=IRSENSOR; /*set the lowest reading to the current reading*/
            lposition=angle; /*set the lowest reading position to the current position*/
        }
    }
    printf("Value %d at %d\n",lreading,lposition); /*print the lowest reading and position*/
    SENSERVO=lposition; /*turn the sensor/fan servo to the lowest position*/
    WSERVO=4000-lposition; /*turn the front wheel to the same position as the servo (i.e.:towards the candle)*/

}

void test()
{
	while (1)
	{
		printf("%d\n",IRSENSOR);
		sleep(.2);
	}
}