// 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; } } }