Пример #1
0
// move base right n steps
void baseLeft(int steps)
{
	digitalWrite(BASEA, LOW);
	digitalWrite(BASEB, HIGH);
	encoder_counter = 0;
	while(encoder_counter < steps + 1)
	{
		delay(1);
	}
	baseStop();
	current_location -= encoder_counter;
	Serial.println("OK");

}
Пример #2
0
void loop() 
{ 

	while (Serial.available())				// If serial data is available on the port
	{				 
		inByte = (char)Serial.read();			 // Read a byte 
		if (echo) Serial.print(inByte);		 // If the scho is on then print it back to the host
		
		if (inByte == '\r')		 // Once a /r is recvd check to see if it's a good command
		{		
			if (echo) Serial.println();	 // Dummy empty line	 
			if (inData == "up") lift_arm();			 
			else if (inData == "downs")			down_seek(50, 0);				// Lower arm until it finds bottom 
			else if (inData == "down")				down(50, ARM_DN_POSITION);		// Blindly lower arm
			else if (inData == "pump on")			pump(ON);						// Start pump
			else if (inData == "pump off")			pump(OFF);						// Stop pump
			else if (inData == "vent")				pump_release();				 	// Stop pump and vent the vacuum
			else if (inData == "left")				baseLeft(16);					// Move base to the left
			else if (inData == "center")			findCenter();		 			// Move base to the right
			else if (inData == "right")			baseRight(BASE_CENTER_POSITION);// Move base to the right
			else if (inData == "stop")				baseStop();						// Stop base
			else if (inData == "home")				left_home();					// Mode base left until it finds home
			else if (inData == "status")			stats();						// Show current status
			else if (inData == "stats")			stats();						// Show current stats
			else if (inData == "get disk")			get_disk();						// Macro to get disk and lift to top
			else if (inData == "load disk")	 	load_disk();					// Macro to get disk and move to center
			else if (inData == "unload disk")	 	unload_disk();					// Macro to get disk and move to center
			else if (inData == "place disk")		place_disk();					// Macro to lower disk into tray
			else if (inData == "debug on")			debug_mode = true;				// turn on debug mode, prints encoder values
			else if (inData == "debug off")	 	debug_mode = false;				// Turn off debug mode
			else if (inData == "echo on")			echo = true;					// Turn on echo mode
			else if (inData == "echo off")		 	echo = false;					// Turn off echo mode		 
			
			else					// The string wasn't recognized so it was a bad command
			{ 
				Serial.print("Error, Unknown Command: ");			 // Show the error
				Serial.println(inData);												 // Echo the command 
			}
			inData = "";			// Clear the buffer (string)
			if (echo) prompt();	 // reprint the prompt

		}
		else inData += inByte;	// If it's not a /r then keep adding to the string (buffer)
	}	

} 
Пример #3
0
//	Return to the home position
void left_home(void)
{
	lift_wrist(WRIST_RAISED_POSITION);
	delay(500);
	
	encoder_counter = 0;	// Reset the location
	lift_arm();
	if (debug_mode) Serial.println("HOMING");
		
	while (current_location > 0)	// If it's not triggered then move the arm
	{	 
		digitalWrite(BASEA, LOW);	 // Moves the arm left
		digitalWrite(BASEB, HIGH);	
	}
	delay(500);			// Wait just a bit to reduce bounce
	baseStop();	// Stop the arm 
	Serial.println("OK");	// Report all is ok
}
Пример #4
0
void progSkills()
{
	//while (SensorValue[bumper] == 0) {}
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
	baseStop();
	// Deploy
	deploy();

	moveLiftAuto(1,3100);
	moveLift(20);
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);
	baseStop();
	/*
	moveStraight(127);
	wait1Msec(200);
	moveStraight(-127);
	wait1Msec(200);
	*/
	moveStraight(127);
	wait1Msec(200);
	intake(-1);
	moveStraight(	-127);
	wait1Msec(200);
	baseStop();

	//intake(-1);
	wait1Msec(2000);

	moveStraight(50);
	wait1Msec(100);
	baseStop();

	intake(1);
	moveLift(-30);
	wait1Msec(500);
	intake(0);

	liftStop();
	moveStraight(-40);
	wait1Msec(200);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)
	{
		while (SensorValue[rightLiftS] < 4000)
		{
			moveLift(-70);
			turnOnSpot(2,90);
		}
		liftStop();
	}
	baseStop();
	intake(1);
	moveStraight(70);
	wait1Msec(1000);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
	{
		turnOnSpot(2,90);
	}
	baseStop();
	moveStraight(127);
	wait1Msec(2000);
	baseStop();

	while(SensorValue[bumper] == 0) {}
	intake(-1);

	while(SensorValue[bumper] == 0) {}
	intake(0);
	// Raise Lift
	// Output

	while(SensorValue[bumper] == 0) {}
	intake (1);
	moveStraight(127);
	wait10Msec(50);
	turnOnSpot(1,60);
	wait10Msec(30);
	turnOnSpot(2,60);
	wait10Msec(30);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
	{
		turnOnSpot(1,90);
	}
	intake(-1);
	wait10Msec(100);
}
Пример #5
0
void redSkyriseAuton()
{
	//while (SensorValue[bumper] == 0) {}
	deploy();
	intake(1);
	SensorValue[rightBackS] = 0;
	while(abs(SensorValue[rightBackS]) < 200)
	{
		moveStraight(127);
	}
	baseStop();
	wait1Msec(1000);

	SensorValue[rightBackS] = 0;
	SensorValue[leftBackS] = 0;
	while(((abs(SensorValue[rightBackS]) + abs(SensorValue[leftBackS]))/2) < 500)
	{
		turnOnSpot(1,90);
	}
	baseStop();

	intake(-1);
	wait1Msec(5000);
	intake(0);

	SensorValue[rightBackS] = 0;
	while(abs(SensorValue[rightBackS]) < 100)
	{
		moveStraight(-90);
	}
	baseStop();

	/*
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
	baseStop();
	// Deploy
	deploy();
	//Raise Lift
	moveLiftAuto(1,3500);
	intake(1);
	moveLift(-1);
	wait1Msec(300);
	moveLift(0);
	moveStraight(127);
	wait1Msec(1000);
	baseStop();
	moveLiftAuto(-1,3450);
	wait1Msec(500);
	moveLiftAuto(1,3000);
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 200)
	{
	moveStraight(-60);
	}
	baseStop();
	SensorValue[rightBackS] = 0;
	SensorValue[leftBackS] = 0;
	while(((abs(SensorValue[rightBackS])+abs(SensorValue[leftBackS]))/2) < 480)
	{
	moveLeftSide(-90);
	}
	moveLeftSide(1);
	wait1Msec(50);
	baseStop();

	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 180)
	{
	moveStraight(70);
	}
	baseStop();

	moveLiftAuto(-1,3550);
	moveStraight(30);
	moveLiftAuto(-1,3900);
	while(abs(SensorValue[rightBackS]) < 200)
	{
	moveStraight(-60);
	}
	baseStop();
	//Move Forward
	//Move Backward
	//Turn around
	//Drop Lift
	//Score Skyrise*/
}