void liftControl(int range){
//motor direction is translated from joystick position
//general motor speed it determined from modifier buttons
motor[ScissorLeft] = scaleForMotor(joystick.joy2_y1, range); //scale -range to +range
motor[ScissorRight] = scaleForMotor(joystick.joy2_y2, range);
}
void driveControl(int range){
//motor speed and direction is directly translated from joystick position
motor[LeftFront] = scaleForMotor(joystick.joy1_y1, range); //scale -range, +range
motor[LeftRear] = scaleForMotor(joystick.joy1_y1, range);
motor[RightFront] = scaleForMotor(joystick.joy1_y2, range);
motor[RightRear] = scaleForMotor(joystick.joy1_y2, range);
}
task main()
{
// zero all motors and servos-----------------------------------------------------------------------------------
servo[clawservo1] = 0; //set servo to zero
servo[clawservo2] = 0; //set servo to zero
wait1Msec(1000); //Pause to let servos move
motor[motorLEFT] = 0; // Turn Motor Off
motor[motorRIGHT] = 0; // Turn Motor Off
motor[motorARM] = 0; // Turn Motor Off
int pos = MIN_POS;
// Start the loop ----------------------------------------------------------------------------------------------
while(true)
{
// Right and Left Joysticks-------------------------------------------------------------------------------------
getJoystickSettings(joystick); // Update Buttons and Joysticks for other functions
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// MotorLEFT's powerlevel is set to the y1 stick's current position.
motor[motorRIGHT] = scaleForMotor(joystick.joy1_y2);// MotorRIGHT's powerlevel is set to the y2 stick's current position.
//--------------------------------------------------------------------------------------------------------------
// BUTTONS - COMMENT OUT UNUSED BUTTONS
if(joy1Btn (1)== 1) // If Joy1-Button (1 blue X) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (2)== 1) // If Joy1-Button (2 green A) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (3)== 1) // If Joy1-Button (3 red B) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (4)== 1) // If Joy1-Button (4 yellow Y) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (5)== 1) // If Joy1- Left Button is pressed:
{
motor[motorARM] = 50; // Motor is run at a power level of 100.
wait1Msec(10); // Wait for motor to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (6)== 1) // If Joy1- Right Button is pressed:
//else if (joy1Btn(6)) // this is how it was written in the sample code
{
pos -= DELTA_POS; // move servo -1 step
if (pos < MIN_POS) // if servo is at minimum...
{
pos = MIN_POS; // reset minimum position
}
servo[servo1] = pos; // move servo to new position
servo[servo2] = pos; // move servo to new position
wait1Msec(10); // wait for servo to move
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (7)== 1) // If Joy1- Left Trigger is pressed:
{
motor[motorARM] = -50; // Motor is run at a power level of -100.
wait1Msec(10); // Wait for motor to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (8)== 1) // If Joy1- Right Trigger is pressed:
//else if (joy1Btn(8)) // this is how it was written in the sample code
{
pos += DELTA_POS; // move servo 1 step
if (pos > MAX_POS) // if servo is at maximum...
{
pos = MAX_POS; // reset maximum position
}
servo[servo1] = pos; // move servo to new position
servo[servo2] = pos; // move servo to new position
wait1Msec(10); // wait for servo to move
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 0) // If 0 button on joy1's D-Pad (up) is pressed:
{
motor[motorARM] = 50; // Motor is run at a power level of 100.
wait1Msec(10); // Wait for motor to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 1) // If 1 button on joy1's D-Pad (up right) is pressed:
{
motor[motorARM] = 50; // Motor is run at a power level of 100.
wait1Msec(10); // Wait for motor to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 2) // If 2 button on joy1's D-Pad (right) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 3) // If 3 button on joy1's D-Pad (right down) is pressed:
{
motor[motorARM] = -50; // MotorA is run at a power level of -100.
wait1Msec(10); // Wait for motor to start
}
//-------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 4) // If 4 button on joy1's D-Pad (down) is pressed:
{
motor[motorARM] = -50; // MotorA is run at a power level of -100.
wait1Msec(10); // Wait for motor to start
}
//-------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 5) // If 5 button on joy1's D-Pad (left down) is pressed:
{
motor[motorARM] = -50; // MotorA is run at a power level of -100.
wait1Msec(10); // Wait for motor to start
}
//------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 6) // If 6 button on joy1's D-Pad (left) is pressed:
{
//PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 7) // If 7 button on joy1's D-Pad (left up) is pressed:
{
motor[motorARM] = 50; // Motor is run at a power level of 100.
wait1Msec(10);
}
//------------------------------------------------------------------------------------------------------------
else // If Joy1-Buttons are NOT pressed:
{
motor[motorARM] = 0; // Turn Arm Motor Off
nxtDisplayTextLine(2, "angle: %.0f", angleFromAccel(accelerometer));
// int angle = angleFromAccel(accelerometer);
// servo[servo1] = (0 + angle);//servo value based on sensor
// servo[servo2] = (0 + angle);//servo value based on sensor
// readAccel(accelerometer);
// if(accelerometer.x>0) // TEMP CODE read sensor
// // TEMP CODE formula for angle
// servo[servo1] = pos + DELTA_POS; // TEMP CODE move servo to new position
// servo[servo2] = pos + DELTA_POS; // TEMP CODE move servo to new position
}
//-----------------------------------------------------------------------------------------------------------------
} // End While Loop ---------------------------------------------------------------------------------------------
} // End Task Main ----------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------
task main()
{ // Start Main Loop
initializeRobot(); // Get ready! see void initalizerobot() above
waitForStart(); // wait for start of tele-op phase From FCS
// Start the loop ----------------------------------------------------------------------------------------------
while(true)
{ // Start While loop
// Right and Left Joysticks-------------------------------------------------------------------------------------
getJoystickSettings(joystick); // Update Buttons and Joysticks
motor[motorLEFT] = scaleForMotor(joystick.joy2_y1);
motor[motorRIGHT] =-scaleForMotor(joystick.joy2_y2);
// Buttons------------------------------------------------------------------------------------------------------
if(joy1Btn (1)== 1) // If Joy1-Button (1 blue X) is pressed:
{ //1open bracket
ENCODER_1_TRGT = ARM_1_COUNT_1 - nMotorEncoder[motorARM1];
ENCODER_2_TRGT = ARM_2_COUNT_1 - nMotorEncoder[motorARM2];
ARM_2_STATE = 1; // Trigger for Stage 2
//---------
if (ENCODER_1_TRGT > DEAD_ARM_1)
{ //2open bracket
PlaySound(soundShortBlip);
wait1Msec(500);
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //3open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] = -scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //3close
ENCODER_1_TRGT = 0;
motor[motorARM1] = 0; // stop motor and hold position
}
if (ENCODER_2_TRGT > DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
ENCODER_2_TRGT = 0;
} // 2close
//---------
if (ENCODER_2_TRGT < -DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = -POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
// servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
// servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
ENCODER_2_TRGT = 0;
}
else if (ENCODER_1_TRGT < -DEAD_ARM_1)
{ //4open
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = -POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //5open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //5close
motor[motorARM1] = 0; // stop motor and hold position
ENCODER_1_TRGT = 0;
} //4close
//---------
motor[motorARM1] = 0; // stop motor and hold position
motor[motorARM2] = 0; // stop motor and hold position
} //1close
// Button 2 Stage 1----------------------------------------------------------------------------------------------
else if(joy1Btn (2)== 1) // If Joy1-Button (2 green B) is pressed:
{ //1open bracket
ENCODER_1_TRGT = ARM_1_COUNT_2 - nMotorEncoder[motorARM1];
ENCODER_2_TRGT = ARM_2_COUNT_2 - nMotorEncoder[motorARM2];
//---------
if (ENCODER_1_TRGT > DEAD_ARM_1)
{while (ENCODER_2_TRGT > DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_1_TRGT = 0;
ENCODER_2_TRGT = 0;
motor[motorARM1] = 0; // stop motor and hold position
}}
//---------
else if (ENCODER_2_TRGT < -DEAD_ARM_2)
{
while (ENCODER_1_TRGT < -DEAD_ARM_1)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = -POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
}
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = -POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //5open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}}
//---------
ENCODER_1_TRGT = 0;
ENCODER_2_TRGT = 0;
motor[motorARM1] = 0; // stop motor and hold position
motor[motorARM2] = 0; // stop motor and hold position
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (3)== 1) // If Joy1-Button (3 red B) is pressed:
{ //1open bracket
ENCODER_1_TRGT=ARM_1_COUNT_3-nMotorEncoder[motorARM1];
ARM_2_STATE = 3; // Trigger for Stage 2
//---------
if (ENCODER_1_TRGT > DEAD_ARM_1)
{ //2open bracket
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //3open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //3close
ENCODER_1_TRGT = 0;
motor[motorARM1] = 0; // stop motor and hold position
} // 2close
//---------
else if (ENCODER_1_TRGT < -DEAD_ARM_1)
{ //4open
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = -POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //5open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //5close
motor[motorARM1] = 0; // stop motor and hold position
ENCODER_1_TRGT = 0;
} //4close
//---------
else if (-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1)
{
motor[motorARM1] = 0; // stop motor and hold position
}
//---------
if (ARM_2_STATE == 3) // If Joy1-Button (1 blue X) is pressed:
{
ENCODER_2_TRGT = ARM_2_COUNT_3 - nMotorEncoder[motorARM2];
if (ENCODER_2_TRGT > DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = POWER_2; // motor ARM is run at high power level
//---------
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_2_TRGT = 0;
}
//---------
else if (ENCODER_2_TRGT < -DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = -POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_2_TRGT = 0;
}
//---------
motor[motorARM2] = 0; // stop motor and hold position
ARM_2_STATE = 0;
}
} //1close
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (4)== 1) // If Joy1-Button (4 yellow Y) is pressed:
{ //1open bracket
ENCODER_1_TRGT=ARM_1_COUNT_4-nMotorEncoder[motorARM1];
ARM_2_STATE = 4; // Trigger for Stage 2
//---------
if (ENCODER_1_TRGT > DEAD_ARM_1)
{ //2open bracket
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //3open - wait for motor to catch up
// servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position
// servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //3close
ENCODER_1_TRGT = 0;
motor[motorARM1] = 0; // stop motor and hold position
} // 2close
//---------
else if (ENCODER_1_TRGT < -DEAD_ARM_1)
{ //4open
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = -POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //5open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //5close
motor[motorARM1] = 0; // stop motor and hold position
ENCODER_1_TRGT = 0;
} //4close
//---------
else if (-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1)
{
motor[motorARM1] = 0; // stop motor and hold position
}
//---------
if (ARM_2_STATE == 4) // If Joy1-Button (1 blue X) is pressed:
{
ENCODER_2_TRGT = ARM_2_COUNT_4 - nMotorEncoder[motorARM2];
if (ENCODER_2_TRGT > DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = POWER_2; // motor ARM is run at high power level
//---------
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_2_TRGT = 0;
}
//---------
else if (ENCODER_2_TRGT < -DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = -POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_2_TRGT = 0;
}
//---------
motor[motorARM2] = 0; // stop motor and hold position
ARM_2_STATE = 0;
}
} //1close
//--------------------------------------------------------------------------------------------------------------
if(joy1Btn (5)== 1) // If Joy1-Left Button is pressed:
{
motor[motorARM1] = POWER_1; // Motor is run at high power level
wait1Msec(10); // Wait for motor to move
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (6)== 1) // If Joy1- Right Button is pressed:
{
POS_CLAW -= DELTA_POS; // move servo -1 step
if (POS_CLAW < MIN_POS_CLAW) // if servo is at minimum...
{
POS_CLAW = MIN_POS_CLAW; // reset minimum position
}
servo[servo1] = POS_CLAW; // move servo to new position
servo[servo2] = POS_CLAW; // move servo to new position
wait1Msec(10); // wait for servo to move
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (7)== 1) // If Joy1- Left Trigger is pressed:
{
motor[motorARM1] = -POWER_1; // Motor is run at -high power level
wait1Msec(10); // Wait for motor to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joy1Btn (8)== 1) // If Joy1- Right Trigger is pressed:
{
POS_CLAW += DELTA_POS; // move servo 1 step
if (POS_CLAW > MAX_POS_CLAW) // if servo is at maximum...
{
POS_CLAW = MAX_POS_CLAW; // reset maximum position
}
servo[servo1] = POS_CLAW; // move servo to new position
servo[servo2] = POS_CLAW; // move servo to new position
wait1Msec(10); // wait for servo to move
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 0) // If 0 button on joy1's D-Pad (up) is pressed:
{
motor[motorARM2] = POWER_2; // Motor 2 power level.
wait1Msec(10); // Wait for motor 2 to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 1) // If 1 button on joy1's D-Pad (up right) is pressed:
{
motor[motorARM2] = POWER_2; // Motor 2 power level.
wait1Msec(10); // Wait for motor 2 to start
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 2) // If 2 button on joy1's D-Pad (right) is pressed:
{
PlaySound(soundShortBlip); // play the sound, 'soundshortblip'
}
//--------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 3) // If 3 button on joy1's D-Pad (rt dn) is pressed:
{
motor[motorARM2] = -POWER_2; // Motor 2 power level
wait1Msec(10); // Wait for motor to start
}
//-------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 4) // If 4 button on joy1's D-Pad (down) is pressed:
{
motor[motorARM2] = -POWER_2; // Motor 2 power level
wait1Msec(10); // Wait for motor to start
}
//-------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 5) // If 5 button on joy1's D-Pad (lt dn) is pressed:
{
motor[motorARM2] = -POWER_2; // Motor 2 power level
wait1Msec(10); // Wait for motor to start
}
//------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 6) // If 6 button on joy1's D-Pad (left) is pressed:
{ //1open bracket
ENCODER_1_TRGT = ARM_1_COUNT_6 - nMotorEncoder[motorARM1];
ENCODER_2_TRGT = ARM_2_COUNT_6 - nMotorEncoder[motorARM2];
while (ENCODER_2_TRGT < -DEAD_ARM_2)
{
nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM2] = -POWER_2; // motor ARM is run at high power level
while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ // wait for motor to catch up
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
}
ENCODER_2_TRGT = 0;
}
while (ENCODER_1_TRGT < -DEAD_ARM_1)
{ //4open
nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM
motor[motorARM1] = -POWER_1; // motor ARM is run at high power level
while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet:
{ //5open - wait for motor to catch up
servo[clawservo1] = SERVO_ANGLE_6; // move servo to new position
servo[clawservo2] = SERVO_ANGLE_6; // move servo to new position
motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
} //5close
ENCODER_1_TRGT = 0;
}
motor[motorARM1] = 0; // stop motor and hold position
motor[motorARM2] = 0; // stop motor and hold position
}
//------------------------------------------------------------------------------------------------------------
else if(joystick.joy1_TopHat == 7) // If 7 button on joy1's D-Pad (lt up) is pressed:
{
motor[motorARM2] = POWER_2; // Motor 2 power level
wait1Msec(10); // Wait for motor to start
}
//------------------------------------------------------------------------------------------------------------
else // If Joy1-Buttons are NOT pressed:
{
motor[motorARM1] = 0; // Turn Arm Motor 1 Off
motor[motorARM2] = 0; // Turn Arm Motor 2 Off
}
//------------------------------------------------------------------------------------------------------------
}//2 End While Loop ----------------------------------------------------------------------------------------
}//1 End Task Main -----------------------------------------------------------------------------------------
