//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();
}
/*************************************
* 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);
}
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 FirstPersonCamera::handleInput(InputMap &input)
{
double x = input.ranges[Input::RANGE_ROTATE_CAMERA_X];
double y = input.ranges[Input::RANGE_ROTATE_CAMERA_Y];
if(input.actions.find(Input::ACTION_MOVE_DISCRETELY) != input.actions.end())
{
moveDiscretely = !moveDiscretely;
lastMoved = 0;
}
if(input.states.find(Input::STATE_CAMERA_ROTATE) != input.states.end())
{
rotateY(-x);
rotateX(y);
}
if(input.states.find(Input::STATE_CAMERA_MOVE_FORWARD) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_FORWARD))
{
moveForwardDiscretely(1);
lastMoved |= MOVED_FORWARD;
}
}
else
{
moveTowardsAt(0.25);
}
}
else
{
lastMoved &= ~MOVED_FORWARD;
}
if(input.states.find(Input::STATE_CAMERA_MOVE_BACK) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_BACK))
{
moveForwardDiscretely(-1);
lastMoved = lastMoved | MOVED_BACK;
}
}
else
{
moveTowardsAt(-0.25);
}
}
else
{
lastMoved &= ~MOVED_BACK;
}
if(input.states.find(Input::STATE_CAMERA_MOVE_LEFT) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_LEFT))
{
strafeRightDiscretely(-1);
lastMoved = lastMoved | MOVED_LEFT;
}
}
else
{
strafeRight(-0.25);
}
}
else
{
lastMoved &= ~MOVED_LEFT;
}
if(input.states.find(Input::STATE_CAMERA_MOVE_RIGHT) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_RIGHT))
{
strafeRightDiscretely(1);
lastMoved = lastMoved | MOVED_RIGHT;
}
}
else
{
strafeRight(0.25);
}
}
else
{
lastMoved &= ~MOVED_RIGHT;
}
if(input.states.find(Input::STATE_CAMERA_MOVE_UP) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_UP))
{
moveUpDiscretely(1);
lastMoved = lastMoved | MOVED_UP;
}
}
else
{
moveEye(glm::vec3(0.0, 0.25, 0.0));
moveAt(glm::vec3(0.0, 0.25, 0.0));
}
}
else
{
lastMoved &= ~MOVED_UP;
}
if(input.states.find(Input::STATE_CAMERA_MOVE_DOWN) != input.states.end())
{
if(moveDiscretely)
{
if(!(lastMoved & MOVED_DOWN))
{
moveUpDiscretely(-1);
lastMoved = lastMoved | MOVED_DOWN;
}
}
else
{
moveEye(glm::vec3(0.0, -0.25, 0.0));
moveAt(glm::vec3(0.0, -0.25, 0.0));
}
}
else
{
lastMoved &= ~MOVED_DOWN;
}
}
