//the keyboard callback to change the values to the transforms
void keyboard(unsigned char key, int x, int y ) {
// printf("%c pressed\n", key);
switch( key ) {
case 'w':
moveForward();
break;
case 's':
moveBackward();
break;
case 'a':
strafeLeft();
break;
case 'd':
strafeRight();
break;
case 32: // space
rise();
break;
case 'c':
fall();
break;
case 'n':
g_shadeType = g_shadeType == NORMAL ? PHONG : NORMAL;
break;
case 'q': case 'Q' :
exit( EXIT_SUCCESS );
break;
}
//glutPostRedisplay();
}
void autonFiveRight(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .6 * clickspermeters, FORWARD, 80);
wait1Msec(20);
driveBackward(100, 100);
StartTask(five);
wrist(127, 600);
wait1Msec(900);
wrist(10, 20);
aArm(1500, RAISE, 120);
drive( .4 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot2] > 1900){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 1000);
wait1Msec(500);
driveBackward(0, 1000);
}
/*************************************
* Definition: Determines what speed of strafe to make based on a measure of center error
*
* Parameters: float value of centerError
*
* Returns: boolean value false if a move was made, true if corrections not required
************************************/
bool Robot::_centerStrafe(float centerError) {
bool success;
if (fabs(centerError) < MAX_STRAFE_CENTER_ERROR) {
success = true;
// we're close enough to centered, so stop adjusting
printf("Center error: |%f| < %f, stop correcting.\n",
centerError,
MAX_STRAFE_CENTER_ERROR);
}
else {
//We needed to make a move, so centering not YET successful
success = false;
// Not using a PID
// Corrections are not inherently linked in time, so use of a PID isn't exactly proper
int strafeSpeed = (int)(10 - 5 * fabs(centerError));
strafeSpeed = Util::capSpeed(strafeSpeed, 10);
if (centerError < 0) {
printf("Center error: %f, move right\n", centerError);
strafeRight(strafeSpeed);
}
else {
printf("Center error: %f, move left\n", centerError);
strafeLeft(strafeSpeed);
}
// we moved, so reset the wheel encoders to ignore
// this movement
_robotInterface->reset_state();
}
return success;
}
void FreeCamera::update(double deltaTime) {
float vel = inputVector.z ? slowVelocity : velocity;
if (inputVector.x < 0)
moveForwards((float)-inputVector.x * vel * (float)deltaTime);
else if (inputVector.x > 0)
moveBackwards((float)inputVector.x * vel * (float)deltaTime);
if (inputVector.y < 0)
strafeLeft((float)-inputVector.y * vel * (float)deltaTime);
else if (inputVector.y > 0)
strafeRight((float)inputVector.y * vel * (float)deltaTime);
}
// autonomous plays are named as follows auton[Score-Result][starting tile]
void autonFiveLeft(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeRight(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wait1Msec(20);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .7 * clickspermeters, FORWARD, 80);
StartTask(five);
wrist(127, 600);
wait1Msec(650);
driveBackward(100, 150);
wrist(10, 20);
driveBackward(100, 300);
//aArm(2100, RAISE, 120);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(100, clickspermeters);
turnLeft(60, 600);
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 300);
wait1Msec(500);
driveBackward(0, 20);
}
task autonomous(){
writeDebugStreamLine("Autonomous Started");
word autonomousMode;
if(SensorValue(autonomousConfig1)==0 && SensorValue(autonomousConfig2)==0){
autonomousMode=5;
//Blue scoring
writeDebugStreamLine("Position: Blue Scoring");
}
else if(SensorValue(autonomousConfig1)==0 && SensorValue(autonomousConfig2)==1){
autonomousMode=7;
//Blue Dead
writeDebugStreamLine("Position: Blue Dead");
}
else if(SensorValue(autonomousConfig1)==1 && SensorValue(autonomousConfig2)==0){
autonomousMode=3;
//Red Scoring
writeDebugStreamLine("Position: Red Scoring");
}
else if(SensorValue(autonomousConfig1)==1 && SensorValue(autonomousConfig2)==1){
autonomousMode=6;
//Red Dead
writeDebugStreamLine("Position: Red Dead");
}
else if(SensorValue(autonomousConfig3)==1 && SensorValue(autonomousConfig1)==0){
autonomousMode=8;
//Marissa Testing
writeDebugStreamLine("Position: Marissa Testing");
}
writeDebugStreamLine("Autonomus mode: %d",(autonomousMode));
if(autonomousMode==1)
{
//Blue Scoring
//Steps for starting on the blue tile in the scoring zone
moveForward(3.25);
turn(120); //Left
stopMotors();
moveForward(0.4);
stopMotors();
moveBackward(2.2);
motor[armMotor] = -127; //Extend Arm
wait1Msec(1000);
stopMotors();
motor[armMotor] = 0; //Stop Arm
moveForward(0.2); //Move a tad forward
wait1Msec(500);
moveBackward(0.2); //Move a tad back
//motor[armMotor] = 127; //Un-Extend Arm
wait1Msec(500);
stopMotors();
motor[armMotor] = 0; //Stop Arm
moveForward(4);
stopMotors();
}
else if(autonomousMode==2) //Depricated
{
//Blue Dead
//Steps for starting on the blue tile in the hanging zone
moveForward(1.75);
turn(-120); //right
stopMotors();
moveForward(2.25);
stopMotors();
motor[armMotor] = -127; //Un-Extend Arm
wait1Msec(500);
motor[armMotor] = 0; //Stop Arm
stopMotors();
moveForward(1.5);
stopMotors();
}
else if(autonomousMode==3)
{
//Red Scoring
//moveForward(3.25);
//turn(120); //left
//stopMotors();
//moveForward(2.25);
//stopMotors();
//motor[armMotor] = 127; //Extend Arm
//wait1Msec(1000);
//stopMotors();
//motor[armMotor] = 0; //Stop Arm
//turn(135); //left
//moveForward(1); //Move forward to home square
//while(SensorValue(unPauseAtonomous)==0){}
//moveForward(1.75); //move towards barrier
moveForward(2);
moveBackward(2);
while(SensorValue(unPauseAtonomous)==0){}
moveForward(3);
moveBackward(3);
while(SensorValue(unPauseAtonomous)==0){}
moveForward(5);
moveBackward(5);
}
else if(autonomousMode==4)
{
//Red Dead
moveForward(1.75);
turn(120); //left
stopMotors();
moveForward(2.25);
stopMotors();
motor[armMotor] = -127; //Un-Extend Arm
wait1Msec(500);
motor[armMotor] = 0; //Stop Arm
stopMotors();
moveForward(1.5);
stopMotors();
}
else if(autonomousMode==5)
{
//Blue scoring
//for if there is a second offensive robot
strafeLeft(1.75);
turn(-120); //right
stopMotors();
moveForward(2.25);
motor[armMotor] = -127; //Un-Extend Arm
wait1Msec(500);
motor[armMotor] = 0; //Stop Arm
stopMotors();
moveForward(1.5);
stopMotors();
}
else if(autonomousMode==6)
{
//Non Match Mode. Skills only
moveForward(1);
strafeLeft(6);
moveBackward(2.5);
turn(-120);
moveForward(6);
}
else if(autonomousMode==7)
{
strafeLeft(2);
moveForward(2);
}
else if(autonomousMode==8)
{
strafeLeft(3);
moveForward(2);
moveBackward(3);
}
}
void autonSkills(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeRight(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wait1Msec(20);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .7 * clickspermeters, FORWARD, 80);
StartTask(five);
wrist(127, 600);
wait1Msec(600);
driveBackward(100, 550);
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(100, clickspermeters);
wait1Msec(20);
turnLeft(60, 600);
wait1Msec(20);
drive( .43 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 400);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
turnLeft(60, 100);
wait1Msec(20);
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2800){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(100);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -35;
motor[leftWrist] = -35;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(2000);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 850);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeRight(127, .9 * clickspermeters);
turnRight(100, 200);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
drive( .4 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(300);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeLeft(127, .5 * clickspermeters);
driveBackward(127 , 700);
wait1Msec(5000);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .25 * clickspermeters, FORWARD, 70);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .7 * clickspermeters, FORWARD, 70);
wait1Msec(20)
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 900);
wait1Msec(10000);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
strafeLeft(127, clickspermeters * .5);
drive( 1.15 * clickspermeters, FORWARD, 70);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
}
void autonomous() {
// redCubeOnPost();
strafeLeft();
}
