// 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");
}
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)
}
}
// 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
}
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);
}
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*/
}