void moveBackDistance(int dist)
{
//Assuming that "dist" is going to be figured out later
//"dist" would be the encoder counts until it stops moving; we can make "onefoot" variables etc. later.
while(RightEncoderCounts <= dist)
{
moveBack(fullspeed);
}
stopWheels();
}
void moveBackTimed(int t, int speed)
{
moveBack(speed);
wait1Msec(t);
stopWheels();
}
void moveForwardTimed(int t, int speed)
{
moveForward(speed);
wait1Msec(t);
stopWheels();
}
void turnLeftTimed(int t)
{
turnLeft();
wait1Msec(t);
stopWheels();
}
void turnRightTimed(int t)
{
turnRight();
wait1Msec(t);
stopWheels();
}
void moveRightTimed(int t, int speed)
{
moveRight(speed);
wait1Msec(t);
stopWheels();
}
/*MAIN*/
int main()
{
//Declarations
int i, j;
int targets;
int center, error, most_blob;
int num_balls = 5;
int dist, prev;
int n = 0;
//Set up GPIO
gpio_export(POWER_LED_PIN);
gpio_set_dir(POWER_LED_PIN, OUTPUT_PIN);
gpio_export(CAMERA_LED_PIN);
gpio_set_dir(CAMERA_LED_PIN, OUTPUT_PIN);
gpio_export(BUTTON_PIN);
gpio_set_dir(BUTTON_PIN, INPUT_PIN);
gpio_export(RELOAD_LED_PIN);
gpio_set_dir(RELOAD_LED_PIN, OUTPUT_PIN);
gpio_export(ARDUINO_PIN);
gpio_set_dir(ARDUINO_PIN, OUTPUT_PIN);
gpio_set_value(ARDUINO_PIN, LOW);
//Set up PWM
initPWM();
usleep(microsecondsToMilliseconds(500));
//Dropper
setPeriod(DROPPER_MOTOR_INDEX, PWM_PERIOD);
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION));
setRun(DROPPER_MOTOR_INDEX, 1);
//Grabber
setPeriod(GRABBER_MOTOR_INDEX, PWM_PERIOD);
setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION);
setRun(GRABBER_MOTOR_INDEX, 1);
//Right wheel
setPeriod(RIGHT_WHEEL_INDEX, PWM_PERIOD);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(RIGHT_WHEEL_INDEX, 0);
//Left wheel
setPeriod(LEFT_WHEEL_INDEX, PWM_PERIOD);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(LEFT_WHEEL_INDEX, 0);
//Roller
setRun(ROLLER_MOTOR_INDEX, 0);
setPeriod(ROLLER_MOTOR_INDEX, PWM_PERIOD);
setDuty(ROLLER_MOTOR_INDEX, PWM_PERIOD*ROLLER_SPEED);
//Set up serial
serial* arduinoSerial;
if (serial_open(&arduinoSerial, ARDUINO_SERIAL_PORT, ARDUINO_BAUD_RATE) == 0){
printf("Opened Ardunio serial port sucessfully\n");
}
else{
printf("Failed to open Arduino serial port\n");
}
/*for(i = 0; i < 50; i++)
{
dist = getDistance(arduinoSerial);
printf("Instensity: %d\n", dist);
usleep(microsecondsToMilliseconds(100));
}
usleep(microsecondsToMilliseconds(5000));*/
//Turn on power LED
gpio_set_value(POWER_LED_PIN, HIGH);
printf("Systems are GO for launch!\n");
waitForButtonPress();
getDistance(arduinoSerial);
//Move to center of the board
goForward();
usleep(microsecondsToMilliseconds(1800));
stopWheels();
//Aim at right target
rotate(180);
if(aimAtSideMostTarget(1))
{
for (; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Move toward left target
rotate(-550);
goForward();
usleep(microsecondsToMilliseconds(2200));
stopWheels();
//Aim at left target
rotate(300);
if(aimAtSideMostTarget(0))
{
for (; num_balls > 1; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(260);
aimAtBiggestTarget();
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
//Move to sideline for reload
rotate(-600);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("%d, ", dist);
usleep(microsecondsToMilliseconds(9));
}
while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1100);
//Turn on reload LED
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
while (n < 3) //big while loop thing!
{
//start from left
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1200));
stopWheels();
rotate(-420);
goForward();
usleep(microsecondsToMilliseconds(590));
stopWheels();
//Aim at left target
if(aimAtSideMostTarget(0))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(420);
goForward();
usleep(microsecondsToMilliseconds(600));
stopWheels();
rotate(-150);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at right target
rotate(400);
goForward();
usleep(microsecondsToMilliseconds(2700));
stopWheels();
rotate(-370);
if(aimAtSideMostTarget(1))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(320);
//usleep(microsecondsToMilliseconds(50));
goForward();
buffer_clear(arduinoSerial);
dist = 0;
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("And another thing....\n");
//printf("%d!!! \n", dist);
usleep(microsecondsToMilliseconds(10));
}while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(-900);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
//start from right
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1350));
stopWheels();
rotate(570);
//Aim at right target
if(aimAtSideMostTarget(1))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(-500);
goForward();
usleep(microsecondsToMilliseconds(4500));
stopWheels();
rotate(650);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at left target
rotate(-200);
if(aimAtSideMostTarget(0))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(-400);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
usleep(microsecondsToMilliseconds(9));
}
while(dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1000);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
} //end of while loop
//Turn off gpio power
gpio_set_value(POWER_LED_PIN, LOW);
gpio_set_value(RELOAD_LED_PIN, LOW);
gpio_set_value(ARDUINO_PIN, LOW);
//Turn off motors
setRun(GRABBER_MOTOR_INDEX, 0);
setRun(DROPPER_MOTOR_INDEX, 0);
setRun(ROLLER_MOTOR_INDEX, 0);
setRun(RIGHT_WHEEL_INDEX, 0);
setRun(LEFT_WHEEL_INDEX, 0);
//Remove GPIO files
//gpio_unexport(PING_PIN);
return 0;
}