All of our archived code from the last 2 seasons can be found at:
http://www.4211thebombers.org/UsefullLinks.html
The "Team 4211's Block Party Code" and "Team 4211's Ring it Up Code" links contain all the autonomous and telop code that we ran at competitions. They have examples of various techniques and strategies for controlling the robot in telop and autonomous, including how to separate the joystick inputs into different threads to allow faster response times.
To more directly answer your question, here is a program with all the basic telop components you need along with various control schemes for driving a robot and for moving a servo. Just uncomment a section you want to try and it will work on a robot.
Code:
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, Right_Drive_Motor, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, Left_Drive_Motor, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C2_1, exampleServo, tServoStandard)
#pragma config(Servo, srvo_S1_C2_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C2_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "joystickdriver.c";
void initializeRobot()
{
// put the code to initialize servo positions in here
// don't put code to move motors in here as that can lead to a DQ
}
task main()
{
initializeRobot(); // initialize robot
waitForStart(); // waits for FCS to start the match
while (1)
{
getJoystickSettings(joystick); // get new message from joystick
// 3 common drive control schemes for differental drive robots are below
/* delete this line and line 29 to run this drive mode
// drive the robot in tank drive style with the joysticks
motor[Left_Drive_Motor]=joystick.joy1_y1;
motor[Right_Drive_Motor]=joystick.joy1_y2;
*/
/* delete this line and line 61 to run this drive mode
// drive the robot with 4 buttons
if (joy1Btn(4)) // drive forward when button 4 is pressed
{
motor[Left_Drive_Motor]=75;
motor[Right_Drive_Motor]=75;
}
else if (joy1Btn(2)) // drive backwards when button 4 is pressed
{
motor[Left_Drive_Motor]=-75;
motor[Right_Drive_Motor]=-75;
}
else if (joy1Btn(1)) // turn left when button 1 is pressed
{
motor[Left_Drive_Motor]=-75;
motor[Right_Drive_Motor]=75;
}
else if (joy1Btn(3)) // turn right when button 2 is pressed
{
motor[Left_Drive_Motor]=75;
motor[Right_Drive_Motor]=-75;
}
else // stop when no button is pressed
{
motor[Left_Drive_Motor]=0;
motor[Right_Drive_Motor]=0;
}
*/
/* delete this line and line 107 to run this drive mode
// drive the robot with the tophat on the left side of the controller
switch(joystick.joy1_TopHat)
{
case 0: // drive forwards when top hat is pushed forwards
motor[Left_Drive_Motor]=75;
motor[Right_Drive_Motor]=75;
break;
case 1: // drive right forwards when top hat is pushed front right
motor[Left_Drive_Motor]=75;
motor[Right_Drive_Motor]=0;
break;
case 2: // turn right when top hat is pushed right
motor[Left_Drive_Motor]=75;
motor[Right_Drive_Motor]=-75;
break;
case 3: // drive back right when top hat is pushed back right
motor[Left_Drive_Motor]=-75;
motor[Right_Drive_Motor]=0;
break;
case 4: // drive backward when top hat is pushed back
motor[Left_Drive_Motor]=-75;
motor[Right_Drive_Motor]=-75;
break;
case 5: // drive back left when top hat is pushed back left
motor[Left_Drive_Motor]=0;
motor[Right_Drive_Motor]=-75;
break;
case 6: // turn left when top hat is pushed left
motor[Left_Drive_Motor]=-75;
motor[Right_Drive_Motor]=75;
break;
case 7: // drive front left when top hat is pushed front left
motor[Left_Drive_Motor]=0;
motor[Right_Drive_Motor]=75;
break;
default: // stop the robot when no top hat direction is pressed
motor[Left_Drive_Motor]=0;
motor[Right_Drive_Motor]=0;
break;
}
*/
// 3 common servo movement controls
/* delete this line and line 132 to run this servo control mode
// move a servo with the joystick
servo[exampleServo]=joystick.joy1_y1;
*/
/* delete this line and line 146 to run this servo control mode
// move a servo to a postition when a button is pressed
if (joy1Btn(5))
{
servo[exampleServo]=0; // change the '0' to whatever position you need when the button is pressed
}
else
{
servo[exampleServo]=255; // change the '0' to whatever position you need when the button is released
}
*/
/* delete this line and line 162 to run this servo control mode
// move a servo to a forward/backward like a motor when a button pair is pressed
if (joy1Btn(5))
{
servo[exampleServo]=ServoValue[exampleServo]+1; // increase the '1' to make the servo change its position faster
}
if (joy1Btn(7))
{
servo[exampleServo]=ServoValue[exampleServo]-1; // increase the '1' to make the servo change its position faster
}
*/
}
}