/* Main Task */
task main() {
bool isAtBeacon;
/* Set up autonomous */
vInitializeRobot();
waitForStart();
/* Put servos into their initial positions */
servo[irServo] = IR_POS_UP;
servo[dividerServo] = REMOVE_THE_DIVIDER;
/* Put motors into their initial positions */
motor[lift] = 100;
vOnLeft(DRIVE_SPEED);
vOnRight(DRIVE_SPEED);
/* Drive until we either find or miss the beacon */
ClearTimer(T1);
do {
if(time1[T1] > 750) motor[lift] = 0;
isAtBeacon = bAtBeacon();
} while((time1[T1] < MAX_SECS) && (!isAtBeacon));
/* Stop the robot */
vOffLeft();
vOffRight();
/* If we found the beacon, score the ring */
if(isAtBeacon){
/* Extend the arm, placing the ring on the peg */
servo[sliderServo] = CONT_FWD;
wait1Msec(2500);
servo[sliderServo] = CONT_OFF;
/* Drive away */
vOnLeft(DRIVE_SPEED);
vOnRight(DRIVE_SPEED);
wait1Msec(500);
vOffLeft();
vOffRight();
/* Retract the arm */
servo[sliderServo] = CONT_BWD;
wait1Msec(2500);
servo[sliderServo] = CONT_OFF;
}
}
task main()
{
//**************************************
//Inital setup
//**************************************
//Setup Static Variables
//Max Servo Angles
int maxDown = 250;
int maxUp = 198;
float maxLeft = 230.0;
float maxRight = 30.0;
//Arm Speeds
int a1dnpwr = 10;
int a1tbpwr = 100;
int a2dnpwr = 10;
int a2tbpwr = 100;
int a3dnpwr = 10;
int a3tbpwr = 100;
//Joystick Deadzone
int deadzone = 15;
//Servo starting positions
float currentLR = 170.0;
int currentUD = maxDown;
//Wheel speed
//**************************************
//End Inital setup
//**************************************
initializeRobot();
//waitForStart(); // wait for start of tele-op phase
while (true)
{
servo[IRservo] = 200;
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific tele-op code here ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//**************************************
//Define control buttons
//**************************************
//Sets Joystick1 button 1 to control Arm 1 going up
getJoystickSettings(joystick); // Get Joystick Variables
int a1upcon = joy2Btn(6);//defines liftarm controls
int a1dncon = joy2Btn(8);
int a2upcon = joy2Btn(5);
int a2dncon = joy2Btn(7);
int a3con = joystick.joy2_TopHat;
int y1 = joystick.joy2_y1; //Motor Left
int y2 = joystick.joy2_y2; //Motor Right
int lrleft = joy2Btn(1);//Servo LR Left
int lrright = joy2Btn(3);//Servo LR Right
int udtoggle = joy2Btn(4);//Servo UD up
int lrpwrdwn = joy2Btn(2);
//**************************************
//End Button Controls
//**************************************
//**************************************
//Process Start
//**************************************
//Arm Control Start
motor[Arm1] = a1upcon == true ? a1tbpwr : a1dncon == true ? -a1dnpwr : 0;
motor[Arm2] = a2upcon == true ? a2tbpwr : a2dncon == true ? -a2dnpwr : 0;
motor[Arm3] = a3con == 0 ? a3tbpwr : a3con == 4 ? -a3dnpwr : 0;
//Arm Control end
if (lrpwrdwn == true)
{
servoChangeRate[leftright] = 0;
}
else
{
servoChangeRate[leftright] = 20;
}
//Tank Drive start
motor[Left1] = y1 > deadzone ? y1 : y1 < -deadzone ? y1/2 : 0;
motor[Right1] = y2 > deadzone ? y2 : y2 < -deadzone ? y2/2 : 0;
motor[Left2] = y1 > deadzone ? y1 : y1 < -deadzone ? y1/2 : 0;
motor[Right2] = y2 > deadzone ? y2 : y2 < -deadzone ? y2/2 : 0;
//Tank Drive end
//LR Servo Control Start
if(lrleft == true && time1[T1] >= 10)
{
if(currentLR < maxLeft)
{
currentLR = currentLR + 01;
}
servo[leftright] = (int) currentLR; //when button 0 (labeled as 1) is pushed move servo to left position
ClearTimer(T1);
}
else if(lrright == true && time1[T1] >= 10)
{
if(currentLR > maxRight)
{
currentLR = currentLR - 01;
}
servo[leftright] = (int) currentLR; //when button 2 (labeled as 3) is pushed move servo to right position
ClearTimer(T1);
}
//LR Servo Control End
//UD Servo Control Start
if (udtoggle == 1 && time1[T2] >= 500)//When the tip control button is pressed
{
if (currentUD == maxDown) //If the number of times tip has been pressed is even then
{
currentUD = maxUp;//set updown servo to the down(flat) position
}
else
{
currentUD = maxDown;//set updown servo to the up(tilted) position
}
ClearTimer(T2);
}
servo[updown] = currentUD;
//UD Servo Control End
if(bAtBeacon())
PlaySound(soundBeepBeep);
// Insert code to have servos and motors respond to joystick and button values
// Look in the ROBOTC samples folder for programs that may be similar to what you want to perform
// You may be able to find "snippets" of code that are similar to the functions that you want to
// perform
}
}
