void init() { bSmartDiagnostics = true; //true to enable smart diagnostic screen bCompetitionMode = true; //true to enable competition mode displaySplash("Mecanum Bot", "", true); bool ok = false; while(!ok) { const int testcount = 2; bool test[testcount] = { errorcheck(1,0,1,MOTORCON), errorcheck(1,0,2,MOTORCON)}; string desc[testcount] = {"MC1-1","MC1-2"}; ok = error_display(test,desc,testcount); if (!ok) { PlayTone(440, 50); if (test[0] == false && test[1] == false){ nxtDisplayCenteredTextLine(7, "Reboot MC!"); } } else { ClearSounds(); } } eraseDisplay(); gyro_init(HTGYRO); wait1Msec(50); nxtbarOn(); return; }
void display_menu(){ //bool we use to keep from redrawing everything run_once=false; drawMenu(); wait1Msec(250); //while we are not pressing enter while(nNxtButtonPressed!=3){ //if we press the left button //and it has been .5 seconds since the last button press //and we have an upper menu to go to if(nNxtButtonPressed==2 && time1[T1] > 500 && selection > 0){ time1[T1] = 0; //reset time 1 so that we don't repeat change_menu(-1); //decrease selection }else if(nNxtButtonPressed==1 && time1[T1] > 500 && selection < 7){ //same as above but with right button and making sure we have a menu item below us change_menu(1); //increase selection time1[T1] = 0; //reset time } } while(nNxtButtonPressed==3); //do not double read enters so hang while holding enter eraseDisplay(); //clear display switch(selection) //get delection and make decision { case 0: //go back current_function=previous_function;//go back to prev page ClearSounds(); //clear sound buffer. not neccesary probably break; case 1: current_function=0;//log break; } wait1Msec(250); // don't immediately move on wait .25 seconds }
sub MTASK_DOTASK(int MTASK_ID){ switch (MTASK_ID) { //******** case MT_DEFAULT: wait1Msec(1); break; //******** case MT_BEEP: PlayTone(200, 12); wait10Msec(120); break; //******** case MT_STOP_BUTTON: if(nNxtButtonPressed==BT_ENTER) { int static TimeDif; TimeDif=time10[T4]; while(nNxtButtonPressed==BT_ENTER){ if(time10[T4]-TimeDif>50) { MV_StopMotors(); ClearSounds(); PlaySound(soundBlip); ESTADO_SET_TARGET(ST_WAIT); break; } } } break; //******** case MT_ACCEL: static int LastTime; if(time100[T4]-LastTime >= 3) { HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]); if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){ if (ACCEL_Filter++ > 2) ACCEL_Rampa = true; }else{ ACCEL_Filter = 0; ACCEL_Rampa = false; } LastTime=time100[T4]; } break; //******** case MT_TOQUE: wait1Msec(1); break; } }
void liftControl() { if (isLiftUp()) { liftUp(); PlaySound(soundUpwardTones); } else if (isLiftDown()) { liftDown(); PlaySound(soundDownwardTones); } else { liftStop(); ClearSounds(); } }
task main() { //wait servo[topServo] = 235; initializeRobot(); StartTask(getHeading); wait1Msec(400); forwardInc(48, 30); //Calculates the positioning of the center goal after 10 position tics at 200msec intervals float a = 0; float b = 0; float c = 0; wait1Msec(400); StartTask(infared); wait1Msec(200); int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a; int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b; for(float i = 0; i <= 10; i++){ if(ir1 < ir2-10){ PlayImmediateTone(440, 10); a += 1; }else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2 PlayImmediateTone(1024, 10); b += 1; }else if(ir1 > ir2+10){ PlayImmediateTone(2024, 10); c += 1; }else{ PlayImmediateTone(1024, 10); //b += 1; } wait1Msec(200); } ClearSounds(); //code for each of the three center positions a=1 b=2 c=3 StopTask(getHeading); wait10Msec(50); StartTask(getHeading); wait1Msec(200); if(a > (b+c)/2){ //PlayImmediateTone(440, 10); currHeading = 0; turnDeg(-20, 25); wait10Msec(50); forwardInc(74, 30); wait10Msec(50); StopTask(getHeading); currHeading = 0; wait1Msec(200); StartTask(getHeading); wait1Msec(100); turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr wait10Msec(25); forwardInc(-22, 25); liftPos(3); servo[topServo] = 210;//original 150 wait10Msec(25); servo[topServo] = 235; wait10Msec(500); liftPos(0); wait10Msec(500); }else if(b > (a+c)/2){ PlayImmediateTone(1024, 10); StopTask(getHeading); currHeading = 0; wait1Msec(200); StartTask(getHeading); wait1Msec(100); turnDeg(180, 25); //do a one 80 StopTask(getHeading); currHeading = 0; wait1Msec(200); StartTask(getHeading); wait10Msec(70); forwardInc(-36, 30);//go back __ inches //StartTask(getHeading); //wait1Msec(100); //turnDeg(20, 25); wait10Msec(200); liftPos(3); servo[topServo] = 210;//original 150 wait10Msec(25); servo[topServo] = 235; wait10Msec(100); liftPos(0); wait10Msec(500); }else if(c > (a+b)/2){ PlayImmediateTone(2024, 10); StopTask(getHeading); currHeading = 0; wait1Msec(200); StartTask(getHeading); wait1Msec(100); turnDeg(-120, 25); StopTask(getHeading); currHeading = 0; wait1Msec(200); StartTask(getHeading); wait10Msec(70); forwardInc(-35, 30); StartTask(getHeading); wait1Msec(100); turnDeg(-32, 25); forwardInc(-4, 25); wait10Msec(50); liftPos(3); servo[topServo] = 210;//original 150 wait10Msec(25); servo[topServo] = 235; wait10Msec(100); liftPos(0); wait10Msec(500); } }
sub MTASK_DOTASK(int MTASK_ID){ switch (MTASK_ID) { //******** case MT_DEFAULT: wait1Msec(1); break; //******** case MT_BEEP: PlayTone(200, 12); wait10Msec(120); break; //******** case MT_STOP_BUTTON: if(nNxtButtonPressed==BT_ENTER) { int static TimeDif; TimeDif=time10[T4]; while(nNxtButtonPressed==BT_ENTER){ if(time10[T4]-TimeDif>50) { MV_StopMotors(); ClearSounds(); PlaySound(soundBlip); ESTADO_SET_TARGET(ST_WAIT); break; } } } break; //******** case MT_ACCEL: if(!getSafe(3)) { hogCPU(); HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]); releaseCPU(); if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){ if (ACCEL_Filter++ > 2){ ACCEL_Rampa = true; ACCEL_Filter = 3; } }else{ if (ACCEL_Filter-- < 2){ ACCEL_Filter = 0; ACCEL_Rampa = false; } } setSafe(3,300); } break; //******** case MT_TOQUE: wait1Msec(1); break; //******** case MT_LL: hogCPU(); LL_Avr = LLreadAverage(PORT_LL); LL_IO = LLreadResult(PORT_LL); releaseCPU(); wait1Msec(5); break; //******** case MT_US: wait10Msec(3); hogCPU(); ReadAllUS_short(PORT_ARD,USwall); releaseCPU(); break; } }
task main() { /* * INITIALIZATION AND SETUP * This first section gets the robot ready to start the match. It sets all the motor and servo * starting positions, and prints some diagnostic information to the NXT and the debug stream. */ // Print a copyright notice to the debug stream string programName = "Autonomous"; printWelcomeMessage(programName, programVersion); writeDebugStreamLine("Getting autonomous settings..."); PlaySound(soundBeepBeep); runMenu(); // Get gameplay decisions from the operators initializeRobot(); // Set the robot to its starting positions // Notify the users that the program is ready and running writeDebugStreamLine("Waiting for start of match..."); nxtDisplayTextLine(0, "2015 Powerstackers"); nxtDisplayCenteredBigTextLine(1, "AUTO"); nxtDisplayCenteredBigTextLine(3, "READY"); PlaySound(soundFastUpwardTones); wait10Msec(200); /* * DEBUG MODE * If debug mode is activated, bypass the waitForStart function. * Display a 3 second audio and visual countdown, and then execute. */ if(debugMode) { ClearSounds(); for(int i = 3; i>0; --i) { nxtDisplayCenteredBigTextLine(3, "IN %d", i); PlaySound(soundException); wait10Msec(100); } } else { // Wait for the "starting gun" from the field control system waitForStart(); } // Notify the users that the program is running nxtDisplayCenteredBigTextLine(3, "RUNNING"); PlaySound(soundUpwardTones); //StartTask(watchMotors); /* * GAMEPLAY * From this point on, the program is split into different sections based on the options chosen in * menu earlier. The two main options are the starting position and the game mode (offense or defense). */ if(startingPosition==STARTING_RAMP) { /* * RAMP POSITION * Starting from this position means that you can only access the rolling goals and the kickstand. * In this position, offensive is mostly the only available game mode. */ // OFFENSIVE MODE if(offenseOrDefense==OFFENSIVE_MODE) { // Go straight down the ramp goTicks(inchesToTicks(-25), 30); turnDegrees(5, 50); goTicks(inchesToTicks(-40), 50); //this will make the robot drop the balls into the rolling goal grabTube(); //this will make the robot turn to move twards the parking zone turnDegrees (-8,preferredTurnSpeed); goTicks(inchesToTicks(12), 75); turnDegrees(-20, preferredTurnSpeed); //this will make the robot move twards the parking zone goTicks (inchesToTicks(78), 75); //this will make the robot turn so it can be in place so it will move in the parking zone turnDegrees (-80,preferredTurnSpeed); goTicks(inchesToTicks(10), 50); dropBall(liftTargetMed); } // END OFFENSE // DEFENSIVE MODE else if(offenseOrDefense==DEFENSIVE_MODE) { // Just go in a straight line down the ramp goTicks(inchesToTicks(80), 50); } // END DEFENSE } // END RAMP START else if(startingPosition==STARTING_FLOOR) { /* * FLOOR POSITION * In this position, you are available to block the other team, or to score in the high goal. You * also have easy access to the kickstand. It is easiest to determine the orientation of the * center tower from this position. */ // Detect and store the center goal position goTicks(inchesToTicks(-30), 100); char goalFacing = findGoalOrientation(); // OFFENSIVE MODE if(offenseOrDefense==OFFENSIVE_MODE) { // Move slightly forwards to get into a better detecting position //goTicks(inchesToTicks(-20), 50); // Make a movement based on the position of the center goal switch (goalFacing) { // GOOD case CENTGOAL_POSITION_A : { // In this position, simply go in a straight line goTicks(inchesToTicks(-10),50); break; } // BAD case CENTGOAL_POSITION_B : { // Turn 45 degrees, tangent to the center structure turnDegrees (45,preferredTurnSpeed); // Move into position in front of the goal goTicks(inchesToTicks(-30),75); // Turn to face the goal turnDegrees (-90,preferredTurnSpeed); writeDebugStreamLine("Done"); break; } // GOOD case CENTGOAL_POSITION_C : { // Turn 90 degrees, parallel to the center structure turnDegrees (-90,preferredTurnSpeed); // Move along the center structure until we're past the end goTicks(inchesToTicks(-38),50); // Turn 90 degrees, tangent to the center structure turnDegrees (90,preferredTurnSpeed); // Move into position in front of the center structure goTicks(inchesToTicks(-62),65); // Turn to face the goal turnDegrees(90,preferredTurnSpeed); break; } default: { // In an ambiguous situtation, assume the goal is in A position goTicks(inchesToTicks(-20),75); writeDebugStreamLine("Done"); break; } } // Print that the alignment is done writeDebugStreamLine("Aligned with center structure"); // Position the robot to drop the ball in the center goal nMotorEncoder[mLift] = 0; moveMotorTo(mLift, liftTargetCent, 75); // Drop the ball in the center goal servo[rTrapDoor]=trapDoorOpenPosition; wait10Msec (500); servo[rTrapDoor]=trapDoorClosedPosition; // Drop down the lift moveMotorTo(mLift, liftTargetBase, 75); // Position the robot correctly to kick the kickstand turnDegrees (90, preferredTurnSpeed); goTicks(inchesToTicks(-15), 75); turnDegrees (90, preferredTurnSpeed); goTicks(inchesToTicks(35), 100); // Go to the robot's ending position } // END OFFENSE // DEFENSIVE MODE else if(offenseOrDefense==DEFENSIVE_MODE) { /* * Move from the starting position to block the opponent's rolling goals */ // Move 2 feet backwards at full power, to get out of the parking zone //goTicks(inchesToTicks(-24), 100); // Turn towards the opponent rolling goals turnDegrees(90, preferredTurnSpeed); // Drive forward and backward to disrupt the opponent 60cm goal goTicks(inchesToTicks(60), 100); goTicks(inchesToTicks(-12), 100); // turn slightly to get the 90cm goals turnDegrees(-45, preferredTurnSpeed); // Move forward and backwards to disrupt the 90cm goal goTicks(inchesToTicks(36), 75); goTicks(inchesToTicks(-36), 100); return; // Turn slightly to face the 30cm goal turnDegrees(-45, preferredTurnSpeed); // Move foward and backward to disrupt the 30cm goal goTicks(inchesToTicks(36), 75); goTicks(inchesToTicks(-4), 100); // Move into position to align on the wall of the ramp turnDegrees(-90, preferredTurnSpeed); //goTicks(inchesToTicks(12), 100); //turnDegrees(90, preferredTurnSpeed); return; // Align ourselves with this wall //wallAlign(ALIGN_FORWARD); /* * BLOCK CETNER GOAL * Move the robot in front of the opponent's center goal */ // Move into position for use to block the center goal goTicks(inchesToTicks(-6), 100); // Move away from the wall turnDegrees(90, preferredTurnSpeed); // Turn to face OUR ramp goTicks(inchesToTicks(-36), 100); // Move foward, past the center goal //turnDegrees(90, preferredTurnSpeed); // Turn parallel to the cetner structure // Move to block the center goal, in one of three positions switch(goalFacing) { // Farthest position case CENTGOAL_POSITION_A: { writeDebugStreamLine("-- BLOCKING GOAL POSITION A --"); goTicks(inchesToTicks(-80), 100); // Move alongside the goal turnDegrees(90, preferredTurnSpeed); // Turn tangent to the center structure goTicks(inchesToTicks(20), 100); // Move in front of the goal break; } // Middle position case CENTGOAL_POSITION_B: { writeDebugStreamLine("-- BLOCKING GOAL POSITION B --"); goTicks(inchesToTicks(-40), 100); // Move alongside the goal turnDegrees(45, preferredTurnSpeed); // Turn tangent to the center structure goTicks(inchesToTicks(20), 100); // Move in front of the goal break; } // Closest position case CENTGOAL_POSITION_C: { writeDebugStreamLine("-- BLOCKING GOAL POSITION C --"); goTicks(inchesToTicks(-20), 100); // Move in front of the goal break; } default: { break; } } } // END DEFENSE } // END FLOOR START // After the program has been carried out, play a cute "done" sound nxtDisplayCenteredBigTextLine(3, "DONE"); PlaySound(soundBeepBeep); wait10Msec(200); } // END