// ======================================================================= // Function to move the robot by the gyro by time sideways V2 // ======================================================================= void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel) { relHeading =0; Current_Angle=0; // reset current angle long adj_power; long adj_deg; int i = 0; wait1Msec(200); motor[LF_motor] = -power; motor[RF_motor] = -power; motor[LB_motor] = power; motor[RB_motor] = power; current_power = power; ClearTimer(T1); bool Done = false; int addPower = 0; while(!Done) { nxtDisplayBigTextLine(4, "L %3d", nrm); if(light == true) { if(nrm > light_threshold) i++; if(i > 10) Done = true; } if(time1[T1] > time) { Done = true; } if(adjust == true) { if(ConstOrRel) adj_deg = (long) constHeading; else adj_deg = (long) relHeading; adj_power = adj_deg*GYRO_PROPORTION; addPower = (current_power*5)/100; if(power > 0) { motor[LF_motor] = -(current_power-addPower); //more motor[RF_motor] = -((current_power+addPower) - adj_power); motor[LB_motor] = (current_power-addPower); //more motor[RB_motor] = ((current_power+addPower) + adj_power); } else { motor[LF_motor] = -(current_power+addPower); motor[RF_motor] = -((current_power-addPower) - adj_power); motor[LB_motor] = (current_power+addPower); motor[RB_motor] = ((current_power-addPower) + adj_power); } } } if(StopWhenDone==true) { motor[LF_motor] = 0; motor[RF_motor] = 0; motor[LB_motor] = 0; motor[RB_motor] = 0; } }
//========================================= // Main Program //========================================= task main() { disableDiagnosticsDisplay(); alive(); initializeRobot(); StartTask(sensors); StartTask(sonarSensors); StartTask(selector); waitForStart(); StartTask(display); constHeading = 0; relHeading = 0; if(calibrate != 2) { gyroCalTime = 3; calibrate = 1; while(calibrate != 2) { EndTimeSlice(); } } constHeading = 0; relHeading = 0; wait1Msec(time_selector*1000); PlaySound(soundBeepBeep); switch(MissionNumber) { case 1: GyroSonar_moveV2(0, SIDE_BACK, sonarLive, 110, -60,true, true, CONSTANT); Gyro_TurnV2(42,-15,CONSTANT); wait1Msec(1000); GyroTime_moveV2(1200,-30,true,false,false); motor[motorA] = -50; wait1Msec(1000); motor[motorA] = -3; GyroTimeS_moveV2(8000,-15,true,true,false,false); if(currDir >= 4 && currDir <= 6) column = 2; if(currDir < 4) column = 1; if(currDir > 6) column = 3; if(column == 1) { GyroTimeS_moveV2(300,15,true,false,true,false); GyroTimeS_moveV2(8000,15,true,true,true,false); } if(column == 3) { GyroTimeS_moveV2(300,-15,true,false,true,false); GyroTimeS_moveV2(8000,-15,true,true,true,false); } break; case 2: while(true) { nxtDisplayBigTextLine(1, "%3d", sonarLive); nxtDisplayBigTextLine(3, "%3d", sonarLive2); } break; case 8: while(true) { //nxtDisplayBigTextLine(1, "S: %3d", sonarLive); //nxtDisplayBigTextLine(3, "S2: %3d", sonarLive2); //nxtDisplayBigTextLine(5, "G: %3f", relHeading); } break; } }
task main() { initializeRobot(); waitForStart(); int curGyro; int oldGyro; oldGyro = getGyroData(S2); movData data; //int offset = getOffset(); bool fieldoriented = true; //bNxtLCDStatusDisplay = false; while( true ) { getJoystickSettings(joystick); if (joystick. joy2_TopHat == 0 || joystick. joy2_TopHat == 1 || joystick. joy2_TopHat == 7) { //lift up Motors_SetSpeed(S1, 1, 1, -100); Motors_SetSpeed(S1, 1, 2, 100); } else if (joystick. joy2_TopHat == 3 || joystick. joy2_TopHat == 4 || joystick. joy2_TopHat == 5){ //lift down Motors_SetSpeed(S1, 1, 1, 60); Motors_SetSpeed(S1, 1, 2, -60); }else { Motors_SetSpeed(S1, 1, 1, 0); Motors_SetSpeed(S1, 1, 2, 0); } if (joy2Btn(4) == 1) //ball intake Motors_SetSpeed(S1, 4, 1, 60); else if (joy2Btn(2) == 1) Motors_SetSpeed(S1, 4, 1, -60); else Motors_SetSpeed(S1, 4, 1, 0); if (joy2Btn(3) == 1) //kickstand knocker-downer servo[stand] = 240; if (joy2Btn(1) == 1) servo[stand] = 150; if (joy2Btn(5) == 1) //dropper servo[ball] = 18; if (joy2Btn(6) == 1) servo[ball] = 160; //if (joy2Btn(8) == 1) // fieldoriented = false; // Drive Base data.xComp = joystick.joy1_x1; data.yComp = joystick.joy1_y1-1; data.rot = joystick.joy1_x2; //data.xComp = 127; //data.yComp = 0; //data.rot = 38; data.xComp = (data.xComp != -128 ? data.xComp : data.xComp+1); data.yComp = (data.yComp != -128 ? data.yComp : data.yComp+1); data.rot = (data.rot != -128 ? data.rot : data.rot+1); //curGyro = getGyroData(S2); //this block of if statements is the controller dead-zone if (data.rot < 10 && data.rot > -10) data.rot = 0; if (data.xComp < 10 && data.xComp > -10) data.xComp = 0; if (data.yComp < 10 && data.yComp > -10) data.yComp = 0; nxtDisplayClearTextLine(2); nxtDisplayClearTextLine(4); nxtDisplayBigTextLine(2, "%d", curGyro); wait1Msec(1); nxtDisplayBigTextLine(4, "%d", oldGyro); //if (fieldoriented) // oldGyro = useGyro(data, oldGyro, curGyro, offset); if (data.rot < 2 && data.rot > -2) data.rot = 0; if (data.xComp < 2 && data.xComp > -2) data.xComp = 0; if (data.yComp < 2 && data.yComp > -2) data.yComp = 0; //byte speed = getSqrt(data.xComp, data.yComp) + abs(data.rot); int speed = (int)(sqrt(data.xComp*data.xComp + data.yComp*data.yComp) + abs(data.rot)); //finds speed (dist formula) if (speed > 127) speed = 127; //Regulates speed drive(data, (byte)speed); } drive(data, 0); }
//==================================================== // Selector //==================================================== task selector() { wait1Msec(1000); while((nNxtButtonPressed != kEnterButton)) { if (nNxtButtonPressed == kLeftButton) { while(nNxtButtonPressed ==kLeftButton){}; MissionNumber = MissionNumber-1; PlaySoundFile("! Click.rso"); } if (MissionNumber < 1) { MissionNumber = 1; } if (MissionNumber > 20) { MissionNumber = 20; } if (nNxtButtonPressed == kRightButton) { while(nNxtButtonPressed ==kRightButton){}; MissionNumber = MissionNumber+1; PlaySoundFile("! Click.rso"); } nxtDisplayBigTextLine(2, "Mission"); nxtDisplayBigTextLine(5, "%2d", MissionNumber); } eraseDisplay(); string tmp = ""; StringFormat(tmp, "%3d", MissionNumber); nxtDisplayBigStringAt(20,18,tmp); while(nNxtButtonPressed == kEnterButton) {} eraseDisplay(); while(nNxtButtonPressed != kEnterButton) { nxtDisplayBigTextLine(6, "SecDelay"); nxtDisplayBigTextLine(4, "%3d", time_selector); if (nNxtButtonPressed == kLeftButton) { while(nNxtButtonPressed ==kLeftButton){}; time_selector = time_selector-1; PlaySoundFile("! Click.rso"); } if (nNxtButtonPressed == kRightButton) { while(nNxtButtonPressed ==kRightButton){} time_selector = time_selector+1; PlaySoundFile("! Click.rso"); } if(time_selector > 30) time_selector = 30; if(time_selector < 0) time_selector = 0; } PlaySound(soundException); wait1Msec(200); eraseDisplay(); while(nNxtButtonPressed == kEnterButton){} while(nNxtButtonPressed != kEnterButton) { nxtDisplayBigTextLine(6, "gyro_cal"); nxtDisplayBigTextLine(4, "%2d", gyroCalTime); if(nNxtButtonPressed == kLeftButton) { while(nNxtButtonPressed == kLeftButton){} gyroCalTime = gyroCalTime-1; PlaySoundFile("! Click.rso"); } if(nNxtButtonPressed == kRightButton) { while(nNxtButtonPressed == kRightButton){} gyroCalTime = gyroCalTime+1; PlaySoundFile("! Click.rso"); } if(gyroCalTime > 15) gyroCalTime = 15; if(gyroCalTime < 0) gyroCalTime = 0; } if(gyroCalTime > 0) calibrate = 1; PlaySound(soundException); eraseDisplay(); }
task main () { int X = 0; memset(data, 0, sizeof(data)); TIRresetSensor(TIR); nxtDisplayCenteredTextLine(0, "Dexter Industries"); nxtDisplayCenteredTextLine(1, "Thermal Infrared"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); eraseDisplay(); // set emissivity for light skin TIRsetEmissivity(TIR, TIR_EM_SKIN_LIGHT); nMotorEncoderTarget[VERTICAL] = 200; motor[VERTICAL] = -20; while((nMotorRunState[VERTICAL] != runStateIdle) && (nMotorRunState[VERTICAL] != runStateHoldPosition)) EndTimeSlice(); nMotorEncoderTarget[HORIZONTAL] = 360; motor[HORIZONTAL] = 20; while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) EndTimeSlice(); wait1Msec(500); nMotorEncoder[HORIZONTAL] = 0; nMotorEncoder[VERTICAL] = 0; PlaySound(soundBeepBeep); while(bSoundActive) EndTimeSlice(); for (int i = 0; i < 80; i++) { wait1Msec(500); X = 0; memset(data, 0, sizeof(data)); nMotorEncoderTarget[HORIZONTAL] = 720; motor[HORIZONTAL] = -40; time1[T1] = 0; while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) { data[X] = TIRreadObjectTemp(TIR); X++; wait1Msec(20); } nxtDisplayBigTextLine(1, "X: %d", X); nxtDisplayBigTextLine(3, "T: %d", time1[T1]); nMotorEncoderTarget[VERTICAL] = 5; motor[VERTICAL] = 20; nMotorEncoderTarget[HORIZONTAL] = 720; motor[HORIZONTAL] = 60; for (int j = 0; j < 200; j++) { if (data[j] != 0) { writeDebugStream("%3.2f,", data[j]); wait1Msec(5); } } writeDebugStreamLine(""); while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) EndTimeSlice(); } bFloatDuringInactiveMotorPWM = true; wait1Msec(10); }
task main() { int elevatorHeightTicks; // Set the following variables to false to hide the standard NXT LCD information bDisplayDiagnostics = false; bNxtLCDStatusDisplay = false; ////////// INITIALIZATIONS ////////// initializeRobot(); // Execute robot initialization routine /////////////// Variables to be used later ///////////////// //int maxArmHeightTicks = inchesToTicks(maxArmHeight); //int minRingPickupHeightTicks = inchesToTicks(minRingPickupHeight); //int WAMservoStep = 3; //Amount to inc1rement the WAM servo position //int WAMservoStep12 = 17; // Move a servo 15 degrees //float maxArmHeight = 45.25; // Maximum Safe Arm Height used during manual control of the arm //float minRingPickupHeight = 8.0; // User selected Autonomous information { selectStartLocation(); // Get the starting location of the robot selectAutoActions(); // Get Autononmous actions switch(AutoActions) { case 0: // No Autonomous break; case 1: // IR Beacon selectRow(); // runs the select row function selectAutoStartDelay(); // select the autonomous start delay break; case 2: // Left Colmnn selectRow(); // runs the select row function selectAutoStartDelay(); // select the autonomous start delay break; case 3: // Center Colmnn selectRow(); // runs the select row function selectAutoStartDelay(); // select the autonomous start delay break; case 4: // Right Colmnn selectRow(); // runs the select row function selectAutoStartDelay(); // select the autonomous start delay break; case 5: // Play Defense selectAutoStartDelay(); // select the autonomous start delay break; case 6: // Left Colmnn selectRow(); // runs the select row function selectAutoStartDelay(); // select the autonomous start delay break; default: // Not a valid selection eraseDisplay(); nxtDisplayCenteredTextLine(1,"DITU SAYS"); nxtDisplayCenteredTextLine(2,"INPUT ERROR"); nxtDisplayCenteredTextLine(4,"TRY AGAIN"); wait1Msec(6000); break; } } eraseDisplay(); // Set the following variables to false to hide the standard NXT LCD information // Show the default FTC Display Information bDisplayDiagnostics = true; bNxtLCDStatusDisplay = true; //bDisplayDiagnostics = false; //bNxtLCDStatusDisplay = false; // Determine the autonomous start delay based on delayAutoStart switch(delayAutoStart) { case 0: // delay start = 0 seconds delayAutoStartValue = 0; break; case 1: // delay start = 1 second delayAutoStartValue = 1000; break; case 2: // delay start = 5 seconds delayAutoStartValue = 5000; break; case 3: // delay start = 10 seconds delayAutoStartValue = 10000; break; case 5: // delay start = 15 seconds delayAutoStartValue = 15000; break; default: // delay start = 0 seconds delayAutoStartValue = 0; break; } // Process robot starting location selection switch(robotStartLocation) { case 0: // Left Wall leftWall = true; break; case 1: // Corner corner = true; break; case 2: // Right Wall rightWall = true; break; default: // Not a valid starting location break; } // Process robot starting location selection switch(Row) { case 0: // Bottom Row pegHeightCmd = 1; break; case 1: // Middle Row pegHeightCmd = 2; break; case 2: // Top Row pegHeightCmd = 3; break; default: // Not a valid starting location break; } waitForStart(); // Wait for the signal to start from the Field Control System //reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually ProcessProto(); switch(AutoActions) { case 0: // No Autonomous break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 1 || Score Ring on the column designated by the IR Beacon===============================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 1: // Score Ring on the column designated by the IR Beacon wait1Msec(delayAutoStartValue); // INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE // move forward, turn 45 degrees, move forward move_forward(24, 2000, 90, 90); wait10Msec(50); //move(1, 0, 1000); clearTimer(T1); //HTIRS2setDSPMode(InfraredSensor, DSP_1200); while (time1[T1] < 500) { irSensorVal = SensorValue[IR]; nxtDisplayBigTextLine(2, "IR: %d", irSensorVal); } ////////////////////////////////////////////////////////////////// switch(irSensorVal) { case 1: turngyro_left(-90.0, 35); wait10Msec(100); move_forward(24, 4000, 90, 90); wait10Msec(100); turngyro_left(83.0, 30); wait10Msec(100); move_forward(.5, 2000, 90, 90); wait1Msec(50); break; //------------------------------------------------------------------ case 2: wait10Msec(100); move_backwards(2, 1000, 90, 90); turngyro_left(-45.0, 50); wait10Msec(50); ClearTimer(T1); nMotorEncoder[Right2] = 0; nMotorEncoder[Left2] = 0; move_forward(56, 4000, 90, 90); ClearTimer(T1); move_backwards(12.5, 2000, 90, 90); wait1Msec(50); break; //----------------------------------------------------- case 3: move_forward(43, 2000, 90, 90); wait10Msec(100); turngyro_left(-90.0, 50); wait10Msec(100); move_forward(2, 1000, 90, 90); wait1Msec(50); break; } elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height wait1Msec(500); // Insert robot move commands here // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeight(elevatorHeightTicks); // Score Ring break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 2 || Score Ring on Left Column==========================================================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 2: // Score Ring on Left Column wait1Msec(delayAutoStartValue); elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height wait1Msec(500); // Insert robot move commands here // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeight(elevatorHeightTicks); // Score Ring break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 3 || Score Ring on Center Column=========================================================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 3: // Score Ring on Center Column wait1Msec(delayAutoStartValue); elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height if (tripped) //If the elevator trips the fail safe, break break; wait1Msec(500); move_forward(60-5, 5000, 90, 90); /////The alignment from the wall that could work...///// //move_forward(24, 4000, 80, 80); //turngyro_left(45, 60, 60); //move_forward(4, 2000, 80, 80); RAM_down(); // Drive forward until the RAM limit switch is activated or time expires ClearTimer(T1); while(ramLimit==0 && time1[T1] < 2500) { // Try to follow the white navigation tape if(SensorValue(colorSensorVal) >= 15) { // The color sensor sees the black platform, command the robot to drift to the right motor[Left1] = 35; motor[Left2] = 35; motor[Right1] = 15; motor[Right2] = 15; } else { // The color sensor sees the white navigation tape, command the robot to drift to the left motor[Left1] = 15; motor[Left2] = 15; motor[Right1] = 35; motor[Right2] = 35; } ProcessProto(); } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = -25; motor[Left2] = -25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = 20; motor[Left2] = 20; motor[Right1] = -20; motor[Right2] = -20; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = 25; motor[Left2] = 25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } /*ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = -20; motor[Left2] = -20; motor[Right1] = -20; motor[Right2] = -20; motor[RAMright] = 95; motor[RAMleft] = 95; }*/ motor[RAMright] = 0; motor[RAMleft] = 0; motor[Left1] = 0; motor[Left2] = 0; motor[Right1] = 0; motor[Right2] = 0; // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring if (tripped) //If the elevator trips the fail safe, break break; wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring if (tripped) //If the elevator trips the fail safe, break break; break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 4 || Score Ring on Right Column=========================================================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 4: // Score Ring on Right Column wait1Msec(delayAutoStartValue); elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height wait1Msec(500); // Insert robot move commands here //asdfjkl; wait1Msec(500); move_forward(12, 5000, 90, 90); turngyro_right(45, 100); move_forward(20, 7000, 80, 80); //RAM_down(); motor[Right1] = 0; motor[Right2] = 0; motor[Left1] = 0; motor[Left2] = 0; // Drive forward until the RAM limit switch is activated or time expires ClearTimer(T1); while(ramLimit==0 && time1[T1] < 2500) { // Try to follow the white navigation tape if(SensorValue(colorSensorVal) >= 15) { // The color sensor sees the black platform, command the robot to drift to the right motor[Left1] = 35; motor[Left2] = 35; motor[Right1] = 15; motor[Right2] = 15; } else { // The color sensor sees the white navigation tape, command the robot to drift to the left motor[Left1] = 15; motor[Left2] = 15; motor[Right1] = 35; motor[Right2] = 35; } ProcessProto(); } ClearTimer(T1); while(time1[T1] < 1500) { motor[Left1] = -25; motor[Left2] = -25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 1500) { motor[Left1] = 20; motor[Left2] = 20; motor[Right1] = -20; motor[Right2] = -20; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 1500) { motor[Left1] = 25; motor[Left2] = 25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = -20; motor[Left2] = -20; motor[Right1] = -20; motor[Right2] = -20; motor[RAMright] = 95; motor[RAMleft] = 95; } motor[RAMright] = 0; motor[RAMleft] = 0; motor[Left1] = 0; motor[Left2] = 0; motor[Right1] = 0; motor[Right2] = 0; // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring if (tripped) //If the elevator trips the fail safe, break break; wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring if (tripped) //If the elevator trips the fail safe, break break; // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeight(elevatorHeightTicks); // Score Ring break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 5 || Play Defense=======================================================================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 5: // Play Defense wait1Msec(delayAutoStartValue); elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height //wait1Msec(500); move_backwards(84.0, 10000, 100, 100); break; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Case: 6 || Score Center & Line Up with the Dispsenser=========================================================// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// case 6: // Score on the center column & line up with the dispenser wait1Msec(delayAutoStartValue); elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height if (tripped) //If the elevator trips the fail safe, break break; wait1Msec(500); move_forward(60-5, 5000, 90, 90); RAM_down(); // Drive forward until the RAM limit switch is activated or time expires ClearTimer(T1); while(ramLimit==0 && time1[T1] < 2500) { // Try to follow the white navigation tape if(SensorValue(colorSensorVal) >= 15) { // The color sensor sees the black platform, command the robot to drift to the right motor[Left1] = 35; motor[Left2] = 35; motor[Right1] = 15; motor[Right2] = 15; } else { // The color sensor sees the white navigation tape, command the robot to drift to the left motor[Left1] = 15; motor[Left2] = 15; motor[Right1] = 35; motor[Right2] = 35; } ProcessProto(); } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = -25; motor[Left2] = -25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = 20; motor[Left2] = 20; motor[Right1] = -20; motor[Right2] = -20; motor[RAMright] = 95; motor[RAMleft] = 95; } ClearTimer(T1); while(time1[T1] < 200) { motor[Left1] = 25; motor[Left2] = 25; motor[Right1] = 25; motor[Right2] = 25; motor[RAMright] = 95; motor[RAMleft] = 95; } motor[RAMright] = 0; motor[RAMleft] = 0; motor[Left1] = 0; motor[Left2] = 0; motor[Right1] = 0; motor[Right2] = 0; // Now that the robot is in the scoring location, raise the elevator to the commanded row elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring if (tripped) //If the elevator trips the fail safe, break break; wait1Msec(500); elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring if (tripped) //If the elevator trips the fail safe, break break; move_backwards(40-5, 5000, 90, 90); turngyro_right(135, 100); move_forward(20-2, 5000, 90, 90); turngyro_left(90, 100); move_forward(10-2, 5000, 90, 90); turngyro_left(90, 100); move_backwards(10-2, 5000, 90, 90); break; default: // Not a valid autonomous action break; } servo[colorSensorServo] = colorServoDefault; wait10Msec(200); // All Done, time to stop all tasks StopAllTasks(); } // End Main Bracket
task main() { int _chVal = 0; // analog input byte inputs = 0; // all digital inputs int value = 0; int touchValue = 0; int touchValue2 = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Proto"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect HTPB"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); // Setup all the digital IO ports as outputs (0x30) 110000 pins B4/B5 = outputs, others are inputs. if (!HTPBsetupIO(HTPB, 0x30)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } while(true) { value = 0; eraseDisplay(); // get the value for ADC channel 0, we want a 10 bit answer int j = 0; for(int i=0; i<5; i++) { _chVal = HTPBreadADC(HTPB, j, 10); switch(j) { case 0: nxtDisplayTextLine(0, "A0: %d", _chVal); if(_chVal>512) value+=(1<<8); break; case 1: nxtDisplayTextLine(1, "A1: %d", _chVal); if(_chVal>512) value+=(1<<7); break; case 2: nxtDisplayTextLine(2, "A2: %d", _chVal); if(_chVal>512) value+=(1<<6); break; case 3: nxtDisplayTextLine(3, "A3: %d", _chVal); if(_chVal>512) value+=(1<<5); break; case 4: nxtDisplayTextLine(4, "A4: %d", _chVal); if(_chVal>512) value+=(1<<4); break; } j++; } inputs = HTPBreadIO(HTPB, 0x3F); nxtDisplayTextLine(5, "D: 0x%x", inputs); value+=(inputs&0x01)<<3; value+=(inputs&0x02)<<1; value+=(inputs&0x04)>>1; value+=(inputs&0x08)>>3; nxtDisplayBigTextLine(6, "%d", mapNineBitToDegrees(value)); touchValue = SensorValue[S2]; touchValue2 = SensorValue[S3]; if(touchValue == 1 && touchValue2 == 1) { HTPBwriteIO(HTPB, 0x30); } // turn on B4 and B5 else if(touchValue == 1) { HTPBwriteIO(HTPB, 0x10); } // turn on B4 else if(touchValue2 == 1) { HTPBwriteIO(HTPB, 0x20); } // turn on B5 else { HTPBwriteIO(HTPB, 0x00); } // turn both B4 and B5 off wait1Msec(50); } }
task main() { while(true){ nxtDisplayBigTextLine(1, "%d", SensorValue[irseek]); } }
// Realiza el seguimiento de la pared lateral situada a la derecha // del robot. El robot mantiene el seguimiento a una distancia de seguridad // hasta que se encuentre con una pared frontal. En este caso se detiene y // realiza un giro de 90 grados hacia la izquierda void track_wall() { float distanciaParaParar = 40; float distanciaAlMuro = 40; float umbral = 0; float v = 0.2; float w = 0; // Para evitar chocar contra un muro frontal while(SensorValue[sonarSensorFrontal] > distanciaParaParar) { AcquireMutex(semaphore_odometry); float x = robot_odometry.x; float y = robot_odometry.y; float th = robot_odometry.th; ReleaseMutex(semaphore_odometry); float sval = SensorValue[sonarSensorLateral]; string temp; StringFormat(temp, "%.2f", sval); nxtDisplayBigTextLine(2, temp); string temp2; StringFormat(temp2, "%.2f", th); nxtDisplayBigTextLine(5, temp2); // Oscila a una distancia segura de la pared lateral // Debe girar a la derecha porque se esta alejando if (SensorValue[sonarSensorLateral] > distanciaAlMuro + umbral) { w = -v; if(th < -0.2){ w = 0; } } // Debe girar a la izquierda porque se esta acercando else if(SensorValue[sonarSensorLateral] < distanciaAlMuro - umbral) { w = v; if(th > 0.2){ w = 0; } } else { if(th > 0.2){ w = -(v*0.9); } else if(th < -0.2){ w = v*0.9; } else{ w = 0; } } setSpeed(v,w); } // Gira al encontrarse con un muro de frente float theta, thetaFinal; float errorTheta = 0.005; setSpeed(0,0); // condicion de parada AcquireMutex(semaphore_odometry); float x = robot_odometry.x; float y = robot_odometry.y; float th = robot_odometry.th; ReleaseMutex(semaphore_odometry); theta = th; thetaFinal = (PI)/2; PlaySoundFile("wilhelmA.rso"); Sleep(1000); v = 0; w = 1.5; setSpeed(v,w); while(abs(theta - thetaFinal) > errorTheta) { AcquireMutex(semaphore_odometry); theta = robot_odometry.th; ReleaseMutex(semaphore_odometry); } }