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);
}
// Wrist and arm code
void armUserControl(){
if(vexRT[Btn6U] == 1 && SensorValue[ArmPot1] > 1850)
// If 6U is pressed, arm goes up
{
motor[leftBottomArm] = 127;
motor[leftTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightTopArm] = 127;
}
else if(vexRT[Btn6D] == 1)
// If 6D is pressed, arm goes down
{
motor[leftBottomArm] = -127;
motor[leftTopArm] = -127;
motor[rightBottomArm] = -127;
motor[rightTopArm] = -127;
}
else
// otherwise, do nothing
{
motor[leftBottomArm] = 0;
motor[leftTopArm] = 0;
motor[rightBottomArm] = 0;
motor[rightTopArm] = 0;
}
if(vexRT[Btn8D] == 1)
// If 6U is pressed, arm goes up
{
isrunning = true;
aArm(MinPot, LOWER, 120);
}
else if(vexRT[Btn8U] == 1){
isrunning = true;
while (SensorValue[ArmPot1] > 1800)// arm up for high goal
{
// while both potentiometers are less than a set position power both arms
motor[rightFrontMotor] = vexRT[Ch3] - vexRT[Ch2] + vexRT[Ch1];
motor[rightBackMotor] = vexRT[Ch3] - vexRT[Ch2] - vexRT[Ch1];
motor[leftFrontMotor] = vexRT[Ch3] + vexRT[Ch2] + vexRT[Ch1];
motor[leftBackMotor] = vexRT[Ch3] + vexRT[Ch2] - vexRT[Ch1];
motor[leftWrist] = -15;
motor[rightWrist] = -15;
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
}
isrunning = false;
}
}
void Game::createWorld()
{
Bone::Ptr aArm(new Bone(3.0f,-15.0f,80.0f));
Bone::Ptr cArm(new Bone(9.0f,-15.0f,80.0f));
aArm->AttachSkin(1.5f,skinTexture);
cArm->AttachSkin(1.0f,skinTexture);
Bone::Ptr bArm(new Bone(5.0f,-89.f,89.f));
bArm->AttachSkin(1.0f,skinTexture);
bArm->AttachSkeletalComponent(std::move(cArm));
aArm->AttachSkeletalComponent(std::move(bArm));
SceneGraph.AttachChild(std::move(aArm));
}
