// Main program that runs when executed
task main()
{
// Initialize the robot
if( !InitializeRobot() )
{
// if init fails, fix robot here
}
///////////////////////////////////////////////////////////////////////////
// TEL-OP PHASE
//
// Place your robot specific tel-op phase code within this while loop
//
///////////////////////////////////////////////////////////////////////////
while( true )
{
getJoystickSettings( joystick );
HandleGameController1(); // Drive the robot function
HandleGameController2();
}
}
task main()
{
if( !InitializeRobot() )
{
}
while( true )
{
getJoystickSettings( joystick );
HandleGameController1();
HandleGameController2();
wait1Msec(10);
}
}
task main()
{
if( !InitializeRobot() )
{
// fix robot code here
}
// waitForStart(); // Wait for the beginning of autonomous phase
// Place your robot specific autonomous code here
while( true )
{
getJoystickSettings( joystick );
HandleJoystick1();
HandleJoystick2();
if( KillRobot() )
{
break;
}
}
}
task main()
{
bool gonext; //used to gate input/output
bool blueSide=false;
bool getBowlBall=false;
bool B_haveBBall=false;
//BEGINNING OF NXT BUTTON INPUT
gonext=false;
nNxtButtonTask=0;
hogCPU();
//SIDE SELECTION
while(!gonext){
ClearTimer(T1);
nxtDisplayCenteredBigTextLine(1, "SIDE");
if(blueSide) nxtDisplayCenteredBigTextLine(3, "BLUE");
else nxtDisplayCenteredBigTextLine(3, "RED");
while(time10[T1]<20);
if(nNxtButtonPressed == 1) blueSide=true;
if(nNxtButtonPressed == 2) blueSide=false;
if(nNxtButtonPressed == 3) gonext=true;
}
if(blueSide) {
ClearTimer(T1);
while(time10[T1]<50); //wait for 1/2 second to make sure button released
}
else {
ClearTimer(T1);
while(time10[T1]<50); //wait for 1/2 second to make sure button released
}
//QUERY GET BOWLING BALL
gonext=false;
while(!gonext){
if(blueSide) {
nxtDisplayCenteredBigTextLine(1, "BLUE");
}
else {
nxtDisplayCenteredBigTextLine(1, "RED");
}
if(getBowlBall) nxtDisplayCenteredBigTextLine(3, "GETBALL");
else nxtDisplayCenteredBigTextLine(3, "STRAIGHT");
ClearTimer(T1);
while(time10[T1]<20);
if(nNxtButtonPressed == 1) {getBowlBall=true; }
if(nNxtButtonPressed == 2) {getBowlBall=false; }
if(nNxtButtonPressed == 3) gonext=true;
}
nNxtButtonTask=-1;
releaseCPU();
InitializeRobot();
waitForStart();
//motor[motorB]=-1;
//motor[motorC]=-1;
//wait1Msec(3000);
ClearTimer(T1); //we can use timer T1 to wait etc
//we should use time100 timer since that allows enough time for autonomous
//time100[T1]=300 is end of autonomous
//advance to goal and turn
if(blueSide){
if(getBowlBall){
distForward(42);wait1Msec(200);
turnRightTo(45);wait1Msec(200);
distForward(16);wait1Msec(200);
distBackward(6);wait1Msec(200);
turnRightTo(125);wait1Msec(200);
//servo[servoGate] = SERVOGATEOPEN;
distForward(12);
//should now be facing ball
goForward(1500);
distForward(60);
turnLeftTo(45);
distForward(28);
}//END of get bowl ball blue
else { //non-ball side: knock crates
/* distForward(45);wait1Msec(200);
//turnRight45();wait1Msec(200);
angleRight(45);wait1Msec(200);
distForward(24);wait1Msec(200);
//could try adding going to parking zone
*/
distForward(150);
}
}
else { //RED SIDE
if(getBowlBall){
distForward(42);wait1Msec(200);
turnLeftTo(-45);wait1Msec(200);
distForward(16);wait1Msec(200);
distBackward(6);wait1Msec(200);
turnLeftTo(-125);wait1Msec(200);
//servo[servoGate] = SERVOGATEOPEN;
distForward(12);
//should now be facing ball
goForward(1500);
distForward(60);
turnRightTo(-45);
distForward(28);
}
else { //non-ball side: knock crates
/*
distForward(45);wait1Msec(200);
//turnLeft45();wait1Msec(200);
angleLeft(45);wait1Msec(200);
distForward(24);wait1Msec(200);
//could try adding going to parking zone
*/
distForward(150);
}
}
////////////
}
//
// Start of main program.
// IF things are set to read from a robot's sensors and not a data log, then this would be the best place
// to actually put in controls for the robot's behaviors and actions. The main SLAM process is called as a
// seperate thread off of this function.
//
int main (int argc, char *argv[])
{
//char command[256], tempString[20];
int x;
//int y;
//double maxDist, tempDist, tempAngle;
int WANDER, EXPLORE, DIRECT_COMMAND;
pthread_t slam_thread;
RECORDING = "";
PLAYBACK = "";
for (x = 1; x < argc; x++) {
if (!strncmp(argv[x], "-R", 2))
RECORDING = "current.log";
if (!strncmp(argv[x], "-r", 2)) {
x++;
RECORDING = argv[x];
}
else if (!strncmp(argv[x], "-p", 2)) {
x++;
PLAYBACK = argv[x];
}
else if (!strncmp(argv[x], "-P", 2))
PLAYBACK = "current.log";
}
fprintf(stderr, "********** Localization Example *************\n");
if (PLAYBACK == "")
if (InitializeRobot(argc, argv) == -1)
return -1;
fprintf(stderr, "********** World Initialization ***********\n");
seedMT(SEED);
// Spawn off a seperate thread to do SLAM
//
// Should use semaphores or similar to prevent reading of the map
// during updates to the map.
//
continueSlam = 1;
pthread_create(&slam_thread, (pthread_attr_t *) NULL, Slam, &x);
fprintf(stderr, "*********** Main Loop (Movement) **********\n");
// This is the spot where code should be inserted to control the robot. You can go ahead and assume
// that the robot is localizing and mapping.
WANDER = 0;
EXPLORE = 0;
DIRECT_COMMAND = 0;
RotationSpeed = 0.0;
TranslationSpeed = 0.0;
// Some very crude commands designed to give manual control over our ATRV Jr
// Removed now for convenience and efficiency, since we're running from data logs right now.
/*
while (1) {
// Was there a character pressed?
// gets(command);
scanf("%s", command);
if (command != NULL) {
if ((PLAYBACK_COMPLETE) || (strncmp(command, "quit", 4) == 0)) {
if (PLAYBACK == "") {
//stop the robot
TranslationSpeed = 0.0;
RotationSpeed = 0.0;
Drive(TranslationSpeed, RotationSpeed);
}
// kill the other thread
continueSlam = 0;
pthread_join(slam_thread, NULL);
return 0;
}
else if (strncmp(command, "speed", 5) == 0) {
strncpy(tempString, index(command, ' '), 10);
TranslationSpeed = atof(tempString);
if (TranslationSpeed > 0.5)
TranslationSpeed = 0.5;
RotationSpeed = 0;
DIRECT_COMMAND = 1;
}
else if (strncmp(command, "turn", 4) == 0) {
strncpy(tempString, index(command, ' '), 10);
RotationSpeed = atof(tempString);
if (RotationSpeed > 0.6)
RotationSpeed = 0.6;
TranslationSpeed = 0;
DIRECT_COMMAND = 1;
}
else if (strncmp(command, "stop", 4) == 0) {
TranslationSpeed = 0.0;
RotationSpeed = 0.0;
Drive(TranslationSpeed, RotationSpeed);
DIRECT_COMMAND = 0;
}
else if (strncmp(command, "print", 5) == 0) {
y = 0;
for (x = 0; x < cur_particles_used; x++)
if (particle[x].probability > particle[y].probability)
y = x;
PrintMap(MAP_PATH_NAME, particle[y].parent, FALSE, -1, -1, -1);
}
else if (strncmp(command, "particles", 9) == 0) {
y = 0;
for (x = 0; x < cur_particles_used; x++)
if (particle[x].probability > particle[y].probability)
y = x;
PrintMap(PARTICLES_PATH_NAME, particle[y].parent, TRUE, -1, -1, -1);
}
else if (strncmp(command, "overlay", 7) == 0) {
y = 0;
for (x = 0; x < cur_particles_used; x++)
if (particle[x].probability > particle[y].probability)
y = x;
PrintMap(MAP_PATH_NAME, particle[y].parent, FALSE, particle[y].x, particle[y].y, particle[y].theta);
}
else if (strncmp(command, "centerx ", 8) == 0) {
strncpy(tempString, index(command, ' '), 10);
scat_center_x = atof(tempString);
}
else if (strncmp(command, "centery ", 8) == 0) {
strncpy(tempString, index(command, ' '), 10);
scat_center_y = atof(tempString);
}
else if (strncmp(command, "radius ", 7) == 0) {
strncpy(tempString, index(command, ' '), 10);
scat_radius = atof(tempString);
}
//else {
//fprintf(stderr, "I don't understand you.\n");
//}
}
if ((DIRECT_COMMAND == 1) && (PLAYBACK == "")) {
Drive(TranslationSpeed, RotationSpeed);
}
else if (PLAYBACK == "") {
// stop the robot
TranslationSpeed = 0.0;
RotationSpeed = 0.0;
Drive(TranslationSpeed, RotationSpeed);
}
}
*/
pthread_join(slam_thread, NULL);
return 0;
}
// Main program that runs when executed
task main()
{
// Initialize the robot
if( !InitializeRobot() )
{
// if init fails, fix robot here
}
// Wait for start of the autonomous phase of the game
// NOTE: waitForStart() function is controlled by the FTC for
// competition. Comment this out for testing directly
// from the RobotC interface.
/* waitForStart(); // Wait for the beginning of autonomous phase
///////////////////////////////////////////////////////////////////////////
// AUTONOMOUS PHASE
//
// Place your robot specific autonomous phase code within this while loop
//
///////////////////////////////////////////////////////////////////////////
while( true )
{
getJoystickSettings( joystick );
// Check if Autonomous phase has ended
if( joystick.StopPgm )
{
break; // break infinite while loop to wait for beginning of tel-op phase
}
// Run Autonomous Robot mode
if( GetIRSeekerData() )
{
if( HandleAutonomousRobot() )
{
wait1Msec( 50 );
}
else
{
// Handle Autonomous robot faults here
}
}
}
// NOTE: waitForStart() function is controlled by the FTC for
// competition. Comment this out for testing directly
// from the RobotC interface.
waitForStart(); // Wait for the beginning of tel-op phase
*/
///////////////////////////////////////////////////////////////////////////
// TEL-OP PHASE
//
// Place your robot specific tel-op phase code within this while loop
//
///////////////////////////////////////////////////////////////////////////
while( true )
{
getJoystickSettings( joystick );
HandleGameController1(); // Drive the robot function
HandleGameController2(); // Pick up and drop game piece function
// Check if we need to kill the robot and exit the program
if( KillRobot() )
{
break;
}
}
}
