Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
void moveBackTimed(int t, int speed)
{
    moveBack(speed);
    wait1Msec(t);
    stopWheels();
}
Exemplo n.º 3
0
void moveForwardTimed(int t, int speed)
{
    moveForward(speed);
    wait1Msec(t);
    stopWheels();
}
Exemplo n.º 4
0
void turnLeftTimed(int t)
{
    turnLeft();
    wait1Msec(t);
    stopWheels();
}
Exemplo n.º 5
0
void turnRightTimed(int t)
{
    turnRight();
    wait1Msec(t);
    stopWheels();
}
Exemplo n.º 6
0
void moveRightTimed(int t, int speed)
{
    moveRight(speed);
    wait1Msec(t);
    stopWheels();
}
Exemplo n.º 7
0
/*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;
}