/*-----------------------------------------------------------------------------*/ task iqMenuTask() { static int select = 0; bool update = true; // This should allow using the exit button but it doesn't seem to work bUserControlsLCDButtons = true; // Run menu forever while( true ) { if( update ) { // Update main menu, only three items iqDisplayLine( LCD_ROW1, select == 0, "Run" ); iqDisplayLine( LCD_ROW2, select == 1, "Configure" ); iqDisplayLine( LCD_ROW3, select == 2, "Reset to Default" ); update = false; } // Check LCD buttons if( nLCDButtons != kButtonNone ) { if( nLCDButtons == kButtonDown ) if( select++ == 2 ) select = 2; if( nLCDButtons == kButtonUp ) if( select-- == 0 ) select = 0; if( nLCDButtons == kButtonSelect ) { // reset if( select == 2 ) iqResetToDefaults(); // Enter configure menu if( select == 1 ) iqMenuConfiguration(); // Enter run mode if( select == 0 ) { // Set everything as enabled robotRunning = true; robotEnabled = true; // Start the control task startTask( iqRunRobotTask, kDefaultTaskPriority + 1 ); // and display information on the LCD while running... iqMenuRun(); // When we return to this point user has indicated program should stop // force task to stop motors and quit robotRunning = false; wait1Msec(100); // task should be done } } // Wait for LCD button release while( nLCDButtons != kButtonNone ) wait1Msec(10); // update display update = true; } // Don't hog cpu wait1Msec(25); } }
int sucker(int speed, int duration) { //positive numbers for out setSuckSpeed(speed); wait1Msec(duration); setSuckSpeed(0); return 1; }
void PIDDriver(short target){ float e, e_dot, old_e = threshold - SensorValue[Light], E = 0; short u; time1[T1] = 0; time1[T2] = 0; time1[T4] = 0; float pre_compass = compass(); bool greyPatch = false; while(true){ if(time1[T1] < timeout){ // T1 for timeout e = threshold - SensorValue[Light]; e_dot = e - old_e; E += e; u = (short)(KP * e + KD * e_dot + KI * E); old_e = e; motor[Left] = cruise_speed - u; motor[Right] = cruise_speed + u; if(SensorValue[Light] < target){ //target can be make more conservative than threshold time1[T1] = 0; } // When grey patch detected if(SensorValue[Light] > 38 && SensorValue[Light] < 43){ if(time1[T4] > 300){ greyPatch = true; stop(); // beep three times PlayTone(1500, 40); wait1Msec(800); PlayTone(1500, 40); wait1Msec(800); PlayTone(1500, 40); wait1Msec(800); control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch while(SensorValue[Light] > threshold){} stop(); time1[T1] = 0; time1[T2] = 0; time1[T3] = 0; nMotorEncoder[Left]=0; nMotorEncoder[Right]=0; pre_compass = compass(); continue; } } else{ time1[T4] = 0; } if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps float cur_compass = compass(); if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){ PlayTone(1175, 20); time1[T3] = 0; // T3 for beepInterval } pre_compass = cur_compass; time1[T2] = 0; // T2 for sampling frequency if(greyPatch){ // log the compass value from grey patch to the other endpoint float tmp = signedCompass(); nxtDisplayStringAt(0,31,"%f", tmp); logCompass(tmp); } } } else{ if(greyPatch){ if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return } else{ turnOnSpot(target, greyPatch); } } } }
task main () { ubyte counter = 0; ubyte oldCounter = 0; ubyte refSignal = 0; ubyte oldRefSignal = 0; ubyte txType = 0; long rawValue = 0; nxtDisplayCenteredTextLine(0, "Mindsensors"); nxtDisplayCenteredBigTextLine(1, "PSP-Nx"); nxtDisplayCenteredTextLine(3, "Test 2"); wait1Msec(2000); PlaySound(soundBeepBeep); while(bSoundActive) EndTimeSlice(); eraseDisplay(); while (true) { // Get the current counter value, wraps at 255 counter = PSPV4readRefSignalCount(PSPNXV4); if (oldCounter != counter) { // This is the command sent by the remote control, can be: // PSPV4_SIGNAL_FAST_REWIND // PSPV4_SIGNAL_FAST_FORWARD // PSPV4_SIGNAL_PLAY // PSPV4_SIGNAL_STOP refSignal = PSPV4readRefSignal(PSPNXV4); // Get the transmitted type. Not very useful in most cases unless // you need to know what type of transmitter sent the command txType = PSPV4readRefTXType(PSPNXV4); // Raw value will also "see" commands from the remote // that are not recognised as "play", "stop", "forward" or "rewind". // You could use this to control additional aspects of your robot! rawValue = PSPV4readRawRefTXValue(PSPNXV4); if (oldRefSignal != refSignal) { PlaySound(soundShortBlip); switch(refSignal) { case PSPV4_SIGNAL_FAST_REWIND: nxtDisplayCenteredBigTextLine(3,"REWIND"); break; case PSPV4_SIGNAL_FAST_FORWARD: nxtDisplayCenteredBigTextLine(3,"FORWARD"); break; case PSPV4_SIGNAL_PLAY: nxtDisplayCenteredBigTextLine(3,"PLAY"); break; case PSPV4_SIGNAL_STOP: nxtDisplayCenteredBigTextLine(3,"STOP"); break; } } // Always display the raw value, even if it's not one of the four // referee signals. Handy if you want to add more commands to your robot! nxtDisplayTextLine(6, "Type: 0x%02X", txType); nxtDisplayTextLine(7, "Raw: 0x%03X", rawValue); // Update the counters and signals oldCounter = counter; oldRefSignal = refSignal; } wait1Msec(50); } }
void Ultra_Seek( int Min, int Max ) { int state = 2; int time; int time_old = 0; int delta_time; float maxVelocity = 180.0; // max angular velocity in deg/sec float GyroOld; float GyroNew; int rotDirection = 1; int motorSpeed = 0; float DegreeGain = 5.0; float degreesToGo; int MAX_GYRO_SPEED = 85; //maximum motor speed allowed in turns int MIN_GYRO_SPEED = 40; //minimum motor speed allowed in turns float Angle; Angle = 0.0; degreesToGo = 0.0; time_old = nPgmTime; GyroOld = 0.0; wait1Msec(100); while(SensorValue[SONAR_1] > Min) { while (SensorValue[SONAR_1] > Min && SensorValue[SONAR_1] < Max) { motor[rightDrive] = 20; motor[leftDrive] = 20; } motor[rightDrive] = 0; motor[leftDrive] = 0; wait10Msec(55); time100[T1] = 0; time100[T2] = 0; while(SensorValue[SONAR_1] > Max) { if(time100[T1] < 30) { motor[rightDrive] = 20; motor[leftDrive] = -20; } if(time100[T2] > 30) { motor[rightDrive] = -20; motor[leftDrive] = 20; } } ClearTimer(T3); time = nPgmTime; // this timer wraps around delta_time = abs(time - time_old); // delta time in ms if (delta_time < 1) // protect against divide by zero { delta_time = 1; } // read the gyro sensor minus the bias offset. GyroBias must be declared and // computed in the calling program. GyroNew = -((float)SensorValue[Gyro] - GyroBias); // limit the gyro to the max achievable by the bot to minimize data spikes. if (abs(GyroNew) > maxVelocity) GyroNew = sgn(GyroNew)*maxVelocity; // deadband for the gyro to eliminate drift due to noise if (abs(GyroNew) <= 0.2) GyroNew = 0.0; // compute the integral of the angular rate using a trapazoidal approximation // http://en.wikipedia.org/wiki/Numerical_integration Angle = Angle + 0.001 * (float)delta_time * 0.5 *(GyroNew + GyroOld); // update the old values for the next time through the loop time_old = time; GyroOld = GyroNew; } } // End of Move()
void deploy() { intake(true); wait1Msec(300); stopIntake(); }
/**Time-Based Intake**/ void intake(int power, int time){ motor[intakeMotor] = power; wait1Msec(time); //Time intake runs motor[intakeMotor] = 0; }
////// PID drive ////// void Drive(int DistanceIN) { long error = 99999; float KP = 0.35; float KD = 2.1; float KI = 1; long Velocity = 0; long Integral = 0; long CurrentDistance; long PreviousDistance = 0; SensorValue(LeftEncoder) = 0; SensorValue(RightEncoder) = 0; SensorValue(TurnGyro) = 0; int Distance; int SpeedCorrect = 0; int MotorSpeed = 0; int cycle = 0; Distance = DistanceIN / 0.024; while (abs(error) > 5) { CurrentDistance = ((SensorValue(RightEncoder)-SensorValue(LeftEncoder))/2); if (Velocity < 10){ if(error > 0){ Integral++; } else { Integral = 0; } } else { Integral = 0; } // P = calculate the error error = Distance - CurrentDistance; // D = Calculate the speed Velocity = CurrentDistance - PreviousDistance; if (SensorValue(TurnGyro) < -2){ // Edit how much is added to change how fast it reacts SpeedCorrect = SpeedCorrect - 0.7; } else if (SensorValue(TurnGyro) > 2){ SpeedCorrect = SpeedCorrect + 0.7; } // Calculates how fast to move the robot MotorSpeed = (error * KP) + (Integral * KI) - (Velocity * KD); BaseSpeed(MotorSpeed - SpeedCorrect, MotorSpeed + SpeedCorrect); writeDebugStreamLine("Cycle: %d", cycle); // Save the distance for the next cycle PreviousDistance = CurrentDistance; wait1Msec(25); } BaseSpeed(0,0); }
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; 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 wait1Msec(delayAutoStartValue); // INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE 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 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 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] < 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; break; 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; 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 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; default: // Not a valid autonomous action break; } servo[colorSensorServo] = colorServoDown; // All Done, time to stop all tasks StopAllTasks(); } // End Main Bracket
task main() { motor[Motor] = 100; wait1Msec(1000); motor[Motor] = 0; }
void moveIntakeTop(int time, int power) { motor[intakeTop] = power; wait1Msec(time); motor[intakeTop] = 0; }
task main() { initializeRobot(); int countline = 0; waitForStart(); // Wait for the beginning of autonomous phase. motor[slide] = 20; wait1Msec(750); motor[slide] = 0; ClearTimer(T1); SensorValue[irSeek] = 0; motor[leftDrive] = 70; motor[rightDrive] = 70; servo[leftgrab] = grabDownPosition; while(time1[T1] < 9000)//stays in loop while less than 9 seconds { if(atLine()) { countline++; // if at line check and see if we are at the IR beacon if break out of loop and go to next loop if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6) { // at Correct Line PlayTone(100,100); break; } else if(countline==1) { motor[rightDrive]= 5;//if it isnt at the beacon and its the first line turn to the right slightly motor[leftDrive] = 0; wait1Msec(500); } else if(countline == 2)/*may need to change*/{ motor[rightDrive] = 4;//if it isn't at the beacon and is at line 2 motor[leftDrive] = 0; wait1Msec(500); } motor[leftDrive] = 75;// drive with 75% power motor[rightDrive] = 75; wait1Msec(500); } } if(time1[T1] < 9000)//while less than 9 seconds and boolean atgood is true { nMotorEncoder[leftDrive] = 0; nMotorEncoder[rightDrive] = 0; if(countline == 1) {//if it is at the beacon and it is at the first line forward(6); //Distance after line till stop stopDrive(); wait1Msec(500); servo[leftgrab] = grabReleasePositionpeg1;//put grabber at position wait1Msec(500); motor[slide] = -50; wait1Msec(1000); motor[slide] = 0; wait1Msec(1000); servo[leftgrab] = grabReleasePositionpeg1 - 15;// move slightly to possible help wait1Msec(1000); motor[leftDrive] = -30; motor[rightDrive] = -30; wait1Msec(1000); motor[leftDrive] = 0; motor[rightDrive] = 0; } else if(countline == 2)// {//if at line 2 and at the beacon forward(7);//Distance after line till stop stopDrive(); wait1Msec(500); servo[leftgrab] = grabReleasePositionpeg2;//move to position for the 2nd peg wait1Msec(500); motor[slide] = -50;//slide over to put the ring on wait1Msec(1000); motor[slide] = 0; wait1Msec(1000); servo[leftgrab] = grabReleasePositionpeg2 + 10;//move slightly to help put the ring on wait1Msec(1000); motor[leftDrive] = -50; motor[rightDrive] = -50; wait1Msec(2000); motor[leftDrive] = 0; motor[rightDrive] = 0; } else if(countline ==3) {//if at line 3 and at beacon forward(6.2);//Distance after line till stop stopDrive(); wait1Msec(500); servo[leftgrab] = grabReleasePositionpeg3;//put grabber to position for 3rd peg wait1Msec(500); motor[slide] = -50;//move to slide the ring on wait1Msec(1000); motor[slide] = 0; wait1Msec(1000); servo[leftgrab] = grabReleasePositionpeg3 + 25; //move slightly to help put ring on wait1Msec(1000); motor[leftDrive] = -50; motor[rightDrive] = -50; wait1Msec(6400); forward(7); } } else stopDrive();//if not below 9 seconds stop nMotorEncoder[leftDrive] = 0; nMotorEncoder[rightDrive] = 0;//clear encoders while (true) {} }
void StopMotors() { driveMotors(0,0); wait1Msec(20); }
//-------------------------------------------------------------------------------------------------------------- task main() { // Start Main Loop initializeRobot(); // Get ready! see void initalizerobot() above waitForStart(); // wait for start of tele-op phase From FCS // Start the loop ---------------------------------------------------------------------------------------------- while(true) { // Start While loop // Right and Left Joysticks------------------------------------------------------------------------------------- getJoystickSettings(joystick); // Update Buttons and Joysticks motor[motorLEFT] = scaleForMotor(joystick.joy2_y1); motor[motorRIGHT] =-scaleForMotor(joystick.joy2_y2); // Buttons------------------------------------------------------------------------------------------------------ if(joy1Btn (1)== 1) // If Joy1-Button (1 blue X) is pressed: { //1open bracket ENCODER_1_TRGT = ARM_1_COUNT_1 - nMotorEncoder[motorARM1]; ENCODER_2_TRGT = ARM_2_COUNT_1 - nMotorEncoder[motorARM2]; ARM_2_STATE = 1; // Trigger for Stage 2 //--------- if (ENCODER_1_TRGT > DEAD_ARM_1) { //2open bracket PlaySound(soundShortBlip); wait1Msec(500); nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //3open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] = -scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //3close ENCODER_1_TRGT = 0; motor[motorARM1] = 0; // stop motor and hold position } if (ENCODER_2_TRGT > DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position ENCODER_2_TRGT = 0; } // 2close //--------- if (ENCODER_2_TRGT < -DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = -POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up // servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position // servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position ENCODER_2_TRGT = 0; } else if (ENCODER_1_TRGT < -DEAD_ARM_1) { //4open nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = -POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //5open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //5close motor[motorARM1] = 0; // stop motor and hold position ENCODER_1_TRGT = 0; } //4close //--------- motor[motorARM1] = 0; // stop motor and hold position motor[motorARM2] = 0; // stop motor and hold position } //1close // Button 2 Stage 1---------------------------------------------------------------------------------------------- else if(joy1Btn (2)== 1) // If Joy1-Button (2 green B) is pressed: { //1open bracket ENCODER_1_TRGT = ARM_1_COUNT_2 - nMotorEncoder[motorARM1]; ENCODER_2_TRGT = ARM_2_COUNT_2 - nMotorEncoder[motorARM2]; //--------- if (ENCODER_1_TRGT > DEAD_ARM_1) {while (ENCODER_2_TRGT > DEAD_ARM_2) { nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_1_TRGT = 0; ENCODER_2_TRGT = 0; motor[motorARM1] = 0; // stop motor and hold position }} //--------- else if (ENCODER_2_TRGT < -DEAD_ARM_2) { while (ENCODER_1_TRGT < -DEAD_ARM_1) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = -POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } } nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = -POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //5open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_2; // move servo to new position servo[clawservo2] = SERVO_ANGLE_2; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves }} //--------- ENCODER_1_TRGT = 0; ENCODER_2_TRGT = 0; motor[motorARM1] = 0; // stop motor and hold position motor[motorARM2] = 0; // stop motor and hold position } //-------------------------------------------------------------------------------------------------------------- else if(joy1Btn (3)== 1) // If Joy1-Button (3 red B) is pressed: { //1open bracket ENCODER_1_TRGT=ARM_1_COUNT_3-nMotorEncoder[motorARM1]; ARM_2_STATE = 3; // Trigger for Stage 2 //--------- if (ENCODER_1_TRGT > DEAD_ARM_1) { //2open bracket nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //3open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //3close ENCODER_1_TRGT = 0; motor[motorARM1] = 0; // stop motor and hold position } // 2close //--------- else if (ENCODER_1_TRGT < -DEAD_ARM_1) { //4open nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = -POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //5open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //5close motor[motorARM1] = 0; // stop motor and hold position ENCODER_1_TRGT = 0; } //4close //--------- else if (-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1) { motor[motorARM1] = 0; // stop motor and hold position } //--------- if (ARM_2_STATE == 3) // If Joy1-Button (1 blue X) is pressed: { ENCODER_2_TRGT = ARM_2_COUNT_3 - nMotorEncoder[motorARM2]; if (ENCODER_2_TRGT > DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = POWER_2; // motor ARM is run at high power level //--------- while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_2_TRGT = 0; } //--------- else if (ENCODER_2_TRGT < -DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = -POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_3; // move servo to new position servo[clawservo2] = SERVO_ANGLE_3; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_2_TRGT = 0; } //--------- motor[motorARM2] = 0; // stop motor and hold position ARM_2_STATE = 0; } } //1close //-------------------------------------------------------------------------------------------------------------- else if(joy1Btn (4)== 1) // If Joy1-Button (4 yellow Y) is pressed: { //1open bracket ENCODER_1_TRGT=ARM_1_COUNT_4-nMotorEncoder[motorARM1]; ARM_2_STATE = 4; // Trigger for Stage 2 //--------- if (ENCODER_1_TRGT > DEAD_ARM_1) { //2open bracket nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //3open - wait for motor to catch up // servo[clawservo1] = SERVO_ANGLE_1; // move servo to new position // servo[clawservo2] = SERVO_ANGLE_1; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //3close ENCODER_1_TRGT = 0; motor[motorARM1] = 0; // stop motor and hold position } // 2close //--------- else if (ENCODER_1_TRGT < -DEAD_ARM_1) { //4open nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = -POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //5open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //5close motor[motorARM1] = 0; // stop motor and hold position ENCODER_1_TRGT = 0; } //4close //--------- else if (-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1) { motor[motorARM1] = 0; // stop motor and hold position } //--------- if (ARM_2_STATE == 4) // If Joy1-Button (1 blue X) is pressed: { ENCODER_2_TRGT = ARM_2_COUNT_4 - nMotorEncoder[motorARM2]; if (ENCODER_2_TRGT > DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = POWER_2; // motor ARM is run at high power level //--------- while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_2_TRGT = 0; } //--------- else if (ENCODER_2_TRGT < -DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = -POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_4; // move servo to new position servo[clawservo2] = SERVO_ANGLE_4; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_2_TRGT = 0; } //--------- motor[motorARM2] = 0; // stop motor and hold position ARM_2_STATE = 0; } } //1close //-------------------------------------------------------------------------------------------------------------- if(joy1Btn (5)== 1) // If Joy1-Left Button is pressed: { motor[motorARM1] = POWER_1; // Motor is run at high power level wait1Msec(10); // Wait for motor to move } //-------------------------------------------------------------------------------------------------------------- else if(joy1Btn (6)== 1) // If Joy1- Right Button is pressed: { POS_CLAW -= DELTA_POS; // move servo -1 step if (POS_CLAW < MIN_POS_CLAW) // if servo is at minimum... { POS_CLAW = MIN_POS_CLAW; // reset minimum position } servo[servo1] = POS_CLAW; // move servo to new position servo[servo2] = POS_CLAW; // move servo to new position wait1Msec(10); // wait for servo to move } //-------------------------------------------------------------------------------------------------------------- else if(joy1Btn (7)== 1) // If Joy1- Left Trigger is pressed: { motor[motorARM1] = -POWER_1; // Motor is run at -high power level wait1Msec(10); // Wait for motor to start } //-------------------------------------------------------------------------------------------------------------- else if(joy1Btn (8)== 1) // If Joy1- Right Trigger is pressed: { POS_CLAW += DELTA_POS; // move servo 1 step if (POS_CLAW > MAX_POS_CLAW) // if servo is at maximum... { POS_CLAW = MAX_POS_CLAW; // reset maximum position } servo[servo1] = POS_CLAW; // move servo to new position servo[servo2] = POS_CLAW; // move servo to new position wait1Msec(10); // wait for servo to move } //-------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 0) // If 0 button on joy1's D-Pad (up) is pressed: { motor[motorARM2] = POWER_2; // Motor 2 power level. wait1Msec(10); // Wait for motor 2 to start } //-------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 1) // If 1 button on joy1's D-Pad (up right) is pressed: { motor[motorARM2] = POWER_2; // Motor 2 power level. wait1Msec(10); // Wait for motor 2 to start } //-------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 2) // If 2 button on joy1's D-Pad (right) is pressed: { PlaySound(soundShortBlip); // play the sound, 'soundshortblip' } //-------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 3) // If 3 button on joy1's D-Pad (rt dn) is pressed: { motor[motorARM2] = -POWER_2; // Motor 2 power level wait1Msec(10); // Wait for motor to start } //------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 4) // If 4 button on joy1's D-Pad (down) is pressed: { motor[motorARM2] = -POWER_2; // Motor 2 power level wait1Msec(10); // Wait for motor to start } //------------------------------------------------------------------------------------------------------------- else if(joystick.joy1_TopHat == 5) // If 5 button on joy1's D-Pad (lt dn) is pressed: { motor[motorARM2] = -POWER_2; // Motor 2 power level wait1Msec(10); // Wait for motor to start } //------------------------------------------------------------------------------------------------------------ else if(joystick.joy1_TopHat == 6) // If 6 button on joy1's D-Pad (left) is pressed: { //1open bracket ENCODER_1_TRGT = ARM_1_COUNT_6 - nMotorEncoder[motorARM1]; ENCODER_2_TRGT = ARM_2_COUNT_6 - nMotorEncoder[motorARM2]; while (ENCODER_2_TRGT < -DEAD_ARM_2) { nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM2] = -POWER_2; // motor ARM is run at high power level while(nMotorRunState[motorARM2] != runStateIdle) // while Motor ARM hasn't reached target yet: { // wait for motor to catch up motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } ENCODER_2_TRGT = 0; } while (ENCODER_1_TRGT < -DEAD_ARM_1) { //4open nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT; // set the target for Motor Encoder of Motor ARM motor[motorARM1] = -POWER_1; // motor ARM is run at high power level while(nMotorRunState[motorARM1] != runStateIdle) // while Motor ARM hasn't reached target yet: { //5open - wait for motor to catch up servo[clawservo1] = SERVO_ANGLE_6; // move servo to new position servo[clawservo2] = SERVO_ANGLE_6; // move servo to new position motor[motorLEFT] = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves } //5close ENCODER_1_TRGT = 0; } motor[motorARM1] = 0; // stop motor and hold position motor[motorARM2] = 0; // stop motor and hold position } //------------------------------------------------------------------------------------------------------------ else if(joystick.joy1_TopHat == 7) // If 7 button on joy1's D-Pad (lt up) is pressed: { motor[motorARM2] = POWER_2; // Motor 2 power level wait1Msec(10); // Wait for motor to start } //------------------------------------------------------------------------------------------------------------ else // If Joy1-Buttons are NOT pressed: { motor[motorARM1] = 0; // Turn Arm Motor 1 Off motor[motorARM2] = 0; // Turn Arm Motor 2 Off } //------------------------------------------------------------------------------------------------------------ }//2 End While Loop ---------------------------------------------------------------------------------------- }//1 End Task Main -----------------------------------------------------------------------------------------
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(time10[T3]-LastTime >= 30) { 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; ACCEL_Filter = 3; } }else{ if (ACCEL_Filter-- < 2){ ACCEL_Filter = 0; ACCEL_Rampa = false; } } LastTime=time10[T3]; } break; //******** case MT_TOQUE: wait1Msec(1); break; //******** case MT_LL: LL_Avr = LLreadAverage(PORT_LL); LL_IO = LLreadResult(PORT_LL); break; //******** case MT_US: wait1Msec(30); ReadAllUS_short(PORT_ARD,USwall); break; } }
task main() { init(); waitForStart(); driveSonar(1,25,80); allStop(); wait1Msec(100); backward(20); wait1Msec(700); sticksDown(); wait1Msec(250); allStop(); wait1Msec(100); raiseLift(100); time1[T1]=0; while (nMotorEncoder[intake] < 410 && time1[T1] < 2500) //while the encoder wheel turns one revolution { times = time1[T1]; } allStop(); wait1Msec(500); //if(time1[T1]>2500) // while(true){ // nxtDisplayCenteredBigTextLine(1,":("); // } releaseBalls(); allStop(); wait1Msec(3000); retainBalls(); allStop(); wait1Msec(750); lowerLift(80); while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution { } allStop(); wait1Msec(500); releaseAutoBall(); wait1Msec(500); motor[intake] = 100; wait1Msec(1000); motor[intake] = 0; turn(0,180,100); allStop(); wait1Msec(100); drive(1,1,75); sticksUp(); drive(0,1,75); time1[T1]=0; while(SensorValue[sonarSensor]>30||time1[T1]<1200){ left(50); } while(SensorValue[sonarSensor]<200){ left(50); } allStop(); wait1Msec(100); turn(0,25,70); allStop(); wait1Msec(100); driveSonar(1,25,50); backward(30); wait1Msec(700); sticksDown(); wait1Msec(250); allStop(); raiseLift(100); while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution { } allStop(); wait1Msec(500); releaseBalls(); allStop(); wait1Msec(3000); retainBalls(); allStop(); wait1Msec(750); lowerLift(80); while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution { } allStop(); wait1Msec(500); }
task main() { RobotState state; initialize(&state); short leftSpeed, rightSpeed; short prevState = INITIALSTATE; bool distLess50; bool irDetected = false; bool sawRedBlue = false; bool goBackward = false; float startAngle = 0; //waitForStart(); wait1Msec(state.delayTime*1000); INITIALDRIVE(); while(true){ //if state changes: stop motors, play tone, reset timers, gyro and lights if (prevState != state.currentState){ if (prevState != PARK_DRIVE2 && prevState != CHECKEND){ stopMotors(); leftSpeed = 0; rightSpeed = 0; } AUDIO_DEBUG(500, 10); time1[T1] = 0; resetGyroAccel(&state); LEDController(0x00); distLess50 = false; startAngle = state.degrees; } getSensors(&state); prevState = state.currentState; if(state.currentState == FINDLINE_TURN){ drive(0, TURNSPEED); if(state.color2 == RED || state.color2 == BLUE){ sawRedBlue = true; } if (sawRedBlue && state.color2 == BLACK){ state.currentState = LINEFOLLOW; } if(abs(state.degrees) > 10){ state.currentState = LINEFOLLOW; } /* state FINDLINE_DRIVE seems unnecessary } else if (state.currentState == FINDLINE_DRIVE) { drive(DRIVESPEED, DRIVESPEED*.95); if (state.irDir == 5 && irDetected == false) { state.currentState = SCOREBLOCK; } else if(state.color2 == RED || state.color2 == BLUE){ state.currentState = LINEFOLLOW; } */ } else if (state.currentState == LINEFOLLOW) { if (state.dist < 50) { distLess50 = true; } if (state.irDir == 5 && irDetected == false) { state.currentState = SCOREBLOCK; } else if (state.dist > 50 && distLess50 && irDetected == true) { state.currentState = GOTOEND; } else if (goBackward){ if (state.color == RED || state.color == BLUE) { leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO; rightSpeed = -DRIVESPEED; }else { leftSpeed = -DRIVESPEED; rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO; } } else { if (state.color2 == RED || state.color2 == BLUE) { leftSpeed = DRIVESPEED*LINEFOLLOWRATIO; rightSpeed = DRIVESPEED; } else { leftSpeed = DRIVESPEED; rightSpeed = DRIVESPEED*LINEFOLLOWRATIO; } } drive(leftSpeed, rightSpeed); } else if (state.currentState == SCOREBLOCK) { irDetected = true; goBackward = true; LEDController(B2); blockScorer(); state.currentState = LINEFOLLOW; } else if (state.currentState == GOTOEND) { drive(-DRIVESPEED, -DRIVESPEED); if(time1[T1] >= 300) { state.currentState = PARK_TURN1; } } else if (state.currentState == PARK_TURN1) { drive(-TURNSPEED, TURNSPEED); if(abs(state.degrees) >= 15){ state.currentState = PARK_DRIVE1; } } else if (state.currentState == PARK_DRIVE1) { drive(-DRIVESPEED, -DRIVESPEED); if(state.color == RED || state.color == BLUE){ state.currentState = PARK_TURN2; } } else if (state.currentState == PARK_TURN2) { DRIVESPECIAL(-TURNSPEED, TURNSPEED); if(abs(state.degrees) >= 22.5){ state.currentState = PARK_DRIVE2; //skip state HARVEST } } else if (state.currentState == HARVEST) { motor[harvester] = 100; DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED); if (time1[T1] >= 500){ leftSpeed = startAngle/abs(startAngle)*TURNSPEED; rightSpeed = -startAngle/abs(startAngle)*TURNSPEED; drive(leftSpeed, rightSpeed); if (abs(startAngle-state.degrees) <= 0.5){ state.currentState = PARK_DRIVE2; } } } else if (state.currentState == PARK_DRIVE2) { DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED); if(abs(state.x_accel) > 35 && state.x_accel < 100){ wait1Msec(20); state.currentState = CHECKEND; } } else if (state.currentState == CHECKEND) { if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){ state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done } else { state.currentState = PARK_DRIVE2; //else you need to do more pushing } } else if (state.currentState == END){ DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED); if(time1[T1] >= 1000){ break; } } else { break; } } }
///////////////////////////////////////////////////////////////////////////////////////////////////// // Main Task // The following is the main code for the autonomous robot operation. // Sequence of Operations: // 1. Detect the IR beam // 2. Drop the block // 3. Backup, Turn, Go Back, Turn // 4. Climb the Ramp // At the end of the autonomous period, the FMS will autonmatically abort execution of the program. ///////////////////////////////////////////////////////////////////////////////////////////////////// task main() { //initialize the servos initializeRobot(); bFloatDuringInactiveMotorPWM = true; // Wait for the beginning of autonomous phase. waitForStart(); servo[servoIRDrop]= 240; //raise the rack so that the robot can move easily RaiseRack(); //move the arm up so that the robot can move easily motor[motorArm] = 65; wait1Msec(1000); motor[motorArm] = 0; //detect the IR beam, we get out when 5 is detected //returns the number of ticks moved int currentTicks = MoveUntilIR(); //move up a bit Move(6, FORWARD, 35); wait1Msec(30); //drop the block DropBlockIR(); //Bring the drop block mechanism higher, //so that its not in the way of the center part of the beam servo[servoIRDrop] = 255 ; wait1Msec(70); //backup //dont go to the start position, start a bit before, so that wall will not be touched nMotorEncoder[motorLeft] = 0; nMotorEncoderTarget[motorLeft] = (currentTicks * -1) + 550; //power the right motor earlier than the left motor motor[motorRight] = -55; wait1Msec(50); //power the left motor motor[motorLeft] = -55; //wait for it go back while ( nMotorRunState[motorLeft] != runStateIdle ) { } //stop the motors motor[motorLeft] = 0; motor[motorRight] = 0; wait1Msec(50); //turnright towards the ramp PointTurnRight(1500); wait1Msec(30); //go back a bit for more clearance Move(50, BACKWARD, 55); wait1Msec(30); PointTurnRight(1900); wait1Msec(30); Move(40, BACKWARD, 55); wait1Msec(30); //brake instead of coast bFloatDuringInactiveMotorPWM = false; //check for other robots if(SensorValue[sonarSensor] <= 75) //if obstruction is present, max power Move(18, BACKWARD, 90); else if(SensorValue[sonarSensor] > 75) Move(18, BACKWARD, 60); //bring it down so it is not in the way of the rack motor servo[servoIRDrop] = 200; wait1Msec(70); // We are done while (true) { } }
/**Time-Based Tread**/ void tread(int power, int time){ motor[treadMotor] = power; wait1Msec(time); //Time intake runs motor[treadMotor] = 0; }
void DropBlockIR() { servo[servoIRDrop]=75; wait1Msec(700); }
// Put the main driver control loop in its own tasks so // the drivers never loses control of the robot! task drive() { // Initialize variables int lastMessage = 0; int leftMotorSpeed = 0; int rightMotorSpeed = 0; int armMotorSpeed = 0; //int specMotorSpeed = 0; int totalMessages = 0; int topSpeed = MOTOR_POWER_DOWN_MAX; int armTopSpeed = 65; //int specTopSpeed = 20; int scoopTopSpeed = 40; int scoopMotorSpeed = 0; while (true) { getJoystickSettings(joystick); if (lastMessage != ntotalMessageCount) { if (true) { ClearTimer(T2); lastMessage = ntotalMessageCount; // New joystick messages have been received! // Set your drive motors based on user input // here. leftMotorSpeed = joystick.joy1_y1 / JOYSTICK_Y1_MAX * topSpeed; // Map the leftMotorSpeed variable to joystick 1_y1 if (abs(leftMotorSpeed) < JOYSTICK_DEAD_ZONE) leftMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone rightMotorSpeed = joystick.joy1_y2 / JOYSTICK_Y1_MAX * topSpeed; // Map the rightMotorSpeed variable to joystick 1_y2 if (abs(rightMotorSpeed) < JOYSTICK_DEAD_ZONE) rightMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone armMotorSpeed = joystick.joy2_y2 / JOYSTICK_Y1_MAX * armTopSpeed; // Map the armMotorSpeed variable to joystick 2_y2 if (abs(armMotorSpeed) < JOYSTICK_DEAD_ZONE) armMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone scoopMotorSpeed = joystick.joy2_y1 / JOYSTICK_Y1_MAX * scoopTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1 if (abs(scoopMotorSpeed) < JOYSTICK_DEAD_ZONE) scoopMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone //specMotorSpeed = joystick.joy1_x1 / JOYSTICK_Y1_MAX * specTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1 //if (abs(specMotorSpeed) < JOYSTICK_DEAD_ZONE) specMotorSpeed = 0; motor[armMotor] = armMotorSpeed; // Set the motor armMotor speed as armMotorSpeed motor[leftMotor] = leftMotorSpeed; // Set the motor leftMotor speed as leftMotorSpeed motor[rightMotor] = rightMotorSpeed; // Set the motor rightMotor speed as rightMotorSpeed motor[scoopMotor] = scoopMotorSpeed; //motor[specMotor] = specMotorSpeed; if (joy1Btn(5) == 1) { // Power up topSpeed = MOTOR_POWER_UP_MAX; } if (joy1Btn(7) == 1) { // Power down topSpeed = MOTOR_POWER_DOWN_MAX; } if (joy2Btn(2) == 1) { // Power up armTopSpeed = ARM_MOTOR_POWER_DOWN; } if (joy1Btn(4) == 1) { // Power up armTopSpeed = ARM_MOTOR_POWER_UP; } if (joy1Btn(4) == 1) { // Nudge forward motor[leftMotor] = NUDGE_POWER; motor[rightMotor] = NUDGE_POWER; wait1Msec(NUDGE_DURATION); motor[leftMotor] = 0; motor[rightMotor] = 0; wait1Msec(NUDGE_DELAY); } if (joy1Btn(2) == 1) { // Nudge backward motor[leftMotor] = NUDGE_POWER * -1; motor[rightMotor] = NUDGE_POWER * -1; wait1Msec(NUDGE_DURATION); motor[leftMotor] = 0; motor[rightMotor] = 0; wait1Msec(NUDGE_DELAY); } if (joy1Btn(0) == 1) { // Nudge left motor[leftMotor] = 0; motor[rightMotor] = NUDGE_POWER; wait1Msec(NUDGE_DURATION); motor[leftMotor] = 0; motor[rightMotor] = 0; wait1Msec(NUDGE_DELAY); } if (joy1Btn(3) == 1) { // Nudge right motor[leftMotor] = NUDGE_POWER; motor[rightMotor] = 0; wait1Msec(NUDGE_DURATION); motor[leftMotor] = 0; motor[rightMotor] = 0; wait1Msec(NUDGE_DELAY); } totalMessages = ntotalMessageCount; } else if (time1[T2] > 200) { // We have not received a packet in over two-tenths of a // second, which probably means communications have been // lost. // Put code to stop all drive motors to avoid // damaging the robot here. PlayImmediateTone(3000, 1); // play a warning tone // Stop motors motor[leftMotor] = 0; motor[rightMotor] = 0; } } } }
task main() { // Gekregen code voor BlueTooth string sig = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; // Einde gekregen code voor BlueTooth while(true) // Main loop { /* * Gekregen code voor BlueTooth * leest berichten die BlueTooth stuurt uit en stopt die in een string. */ nSizeOfMessage = cCmdMessageGetSize(INBOX); if (nSizeOfMessage > kMaxSizeOfMessage) nSizeOfMessage = kMaxSizeOfMessage; if (nSizeOfMessage > 0){ nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; string s = ""; stringFromChars(s, (char *) nRcvBuffer); displayCenteredBigTextLine(4, s); /* * Einde gekregen code voor BlueTooth */ sig = nRcvBuffer;// zet bluetoothsignaal in variabele //writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven // naar voren rijden zonder lijndetectie if(sig == "U"){ setMotor(motorA, 30); setMotor(motorB, 30); wait1Msec(10); } // naar achter rijden zonder lijndetectie if(sig=="D"){ setMotor(motorA, -30); setMotor(motorB, -30); wait1Msec(10); } // maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten) if(sig=="L"){ setMotorTarget(motorA, 0, 0); setMotorTarget(motorB, 360, 30); waitUntilMotorStop(motorB); startTask(Rijden); k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 } // maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten) if(sig=="R"){ setMotorTarget(motorA, 360, 30); setMotor(motorB, 0); waitUntilMotorStop(motorA); startTask(Rijden); k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 } // rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten) if(sig=="F"){ setMotorTarget(motorA, 180, 30); setMotorTarget(motorB, 180, 30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); k=1; // zet anti-lijnhump op 1 startTask(Rijden); } // Stoppen if(sig=="A"){ setMultipleMotors(motorA, motorB, 0); stopTask(Rijden); clearSounds(); } // Zet lijndetectie aan if(sig=="C"){ k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 startTask(Rijden); } /* * nog 1 knop ongebruikt (B). */ // Na elk commando, wacht 0,1s. wait1Msec(100); } } }
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); } }
task Rijden() { // Geluid dat de robot maakt tijdens het rijden, in loop. k = 0; // Reset kruispuntboolean naar 0 //l = 0; int var = 0; // variabele die naar een waarde wordt geset afhankelijk van welke sensor een signaal geeft. while(true){ playSoundFile("hehe.rso"); // Linker sensor: Trigger wanneer de zwart-wit sensor (links) te weinig wit registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar links. if(SensorValue[WIT] < 55) var=1; // Rechter sensor: Trigger wanneer de kleurensensor (rechts) zwart registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar rechts. if((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor)) var=2; // kruispuntherkenning: Trigger wanneer zowel de kleurensensor als de zwartwit sensor getriggerd worden. Voer case 3 uit: stop, rij achteruit, wacht op commando. if(((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))&&((SensorValue[WIT] <55))) var=3; // Tegenovergestelde van kruispuntherkenning: Trigger als de sensoren alleen wit registreren. Voer case 4 uit: rij naar voren. if(((SensorValue[ZWART]!=blackcolor)&&(SensorValue[ZWART]!=greencolor))&&((SensorValue[WIT] >55))) var=4; // Obstakelsensor: Trigger wanneer de sonar een obstakel registreert. Voer case 5 uit: geef geluid en stop. if (SensorValue[sonar] <30) var=5; switch(var) { case 1: //stuur naar links setMotor(motorB,30); setMotor(motorA,-5); var=0; k=0; break; case 2: //stuur naar rechts setMotor(motorB,-5); setMotor(motorA,30); var=0; k=0; break; case 3: //stoppen, ga achteruit, wacht op BlueTooth signaal setMotor(motorB,0); setMotor(motorA,0); wait1Msec(100); clearSounds(); setMotorTarget(motorA,85,-30); // ga achteruit setMotorTarget(motorB,85,-30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); waitUntil(k==1); // wacht op een BlueTooth signaal dat links, rechts of vooruit zegt en k=1 triggert. var=0; k=0; break; case 4: // rij naar voren setMotor(motorB,30); setMotor(motorA,30); var=0; k=0; break; case 5: // als sonar wat ziet: schreeuw, rem rustig af en roteer 180*. clearSounds(); playSoundFile("scream4.rso"); for (int i=3000;i>0 ; i--) { setMotor(motorA,i/100); setMotor(motorB,i/100); } //waitUntil(l==1); //nxtDisplayTextLine(1,"%d",i); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); // oorspronkelijk hadden we code om een object te ontwijken, zowel linksom als rechtsom, // met als alternatief om 180* te draaien. Dit kregen we echter niet op tijd volledig // aan de praat, waardoor we hebben gekozen om hardcoded 180* te draaien. // zie hiervoor de AvoidObject.c en AvoidObject.h files. // //AvoidMain(); //waitUntil(avoidDone==1); //waitUntil(l==1); //avoidDone = 0; // // einde ontwijkcode // hardcoded rotate robot 180* //setMotorTarget(motorA, 360, 30); // rechtsom draaien //setMotorTarget(motorB, 360, -30); //waitUntilMotorStop(motorA); //waitUntilMotorStop(motorB); // end hardcoded rotate robot 180* setMotorTarget(motorA, 360, 30); waitUntilMotorStop(motorA); setMotorTarget(motorA, 135, 30); setMotorTarget(motorB, 135, 30); waitUntilMotorStop(motorA); setMotorTarget(motorB, 360,30); waitUntilMotorStop(motorB); setMotorTarget(motorA, 540, 30); setMotorTarget(motorB, 540, 30); waitUntilMotorStop(motorA); setMotorTarget(motorB, 360,30); waitUntilMotorStop(motorB); setMotorTarget(motorA, 135, 30); setMotorTarget(motorB, 135, 30); waitUntilMotorStop(motorA); var=0; l=0; break; } } }
void lock_msec(int speed, int duration){ setArmSpeed(speed); wait1Msec(duration); setArmSpeed(0); }
void turnRobot(float degrees, int direction, int turntype) { float arc = degrees / 360; float circumference, inches; int ticks; static int deltaLeft = 0, deltaRight = 0; if (TIGHT_TURNS) { circumference = PI * WHEEL_BASE; } else { circumference = 2 * PI * WHEEL_BASE; } inches = circumference * arc; ticks = inchesToTicks(inches); nMotorEncoder[drive1] = 0; nMotorEncoder[drive2] = 0; if (PID_CONTROL == false) { if (turntype == RIGHT) { motor[drive2] = TURN_POWERL * direction; if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE; } else { motor[drive1] = TURN_POWERR * direction; if (TIGHT_TURNS) motor[drive2] = TURN_POWERL * direction * REVERSE; } while (true) { if (abs(nMotorEncoder[drive1]) >= ticks) break; if (abs(nMotorEncoder[drive2]) >= ticks) break; } } else { if (turntype == RIGHT) { nMotorEncoderTarget[drive2] = ticks * direction; if (TIGHT_TURNS) nMotorEncoderTarget[drive1] = ticks * direction * REVERSE; motor[drive2] = TURN_POWERL * direction; if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE; } else { nMotorEncoderTarget[drive1] = ticks * direction; if (TIGHT_TURNS) nMotorEncoderTarget[drive2] = ticks * direction * REVERSE; motor[drive1] = TURN_POWERL * direction; if (TIGHT_TURNS) motor[drive2] = TURN_POWERR * direction * REVERSE; } while (true) { TNxtRunState state1, state2; // 0 = runStateIdle // 1 = runStateHoldPosition // 16 = runStateRampUp // 32 = runStateRunning // 64 = runStateRampDown state1 = nMotorRunState[drive1]; state2 = nMotorRunState[drive2]; if (state1 == runStateIdle && state2 == runStateIdle) break; } } motor[drive1] = 0; motor[drive2] = 0; metricsDisplay(ticks, ticks); deltaLeft = nMotorEncoder[drive2] - ticks; deltaRight = nMotorEncoder[drive1] - ticks; nMotorEncoder[drive1] = 0; nMotorEncoder[drive2] = 0; if (PLAY_SOUNDS == true) { PlaySound(soundBeepBeep); } wait1Msec(DRIVE_DELAY); return; }
void drive_msec(int speed, int duration) { setDriveSpeed(speed); wait1Msec(duration); killdrive; }
task main () { bool selected_50hz = true; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Color V2"); nxtDisplayCenteredTextLine(4, "Config operating"); nxtDisplayCenteredTextLine(5, "frequency to"); nxtDisplayCenteredTextLine(6, "50 or 60 Hz"); wait1Msec(2000); eraseDisplay(); nxtDisplayCenteredTextLine(0, "Use arrow keys"); nxtDisplayCenteredTextLine(1, "to select a"); nxtDisplayCenteredTextLine(2, "frequency"); nxtDisplayCenteredBigTextLine(4, "50 60"); nxtDisplayCenteredTextLine(6, "[enter] to set"); nxtDisplayCenteredTextLine(7, "[exit] to cancel"); nxtDrawRect(19, 34, 44, 16); while (true) { // Do nothing while no buttons are pressed while (nNxtButtonPressed == kNoButton) { wait1Msec(1); } switch (nNxtButtonPressed) { // if the left button is pressed, set the sensor for 50Hz case kLeftButton: if (selected_50hz) { PlaySound(soundBlip); while(bSoundActive) {wait1Msec(1);} } else { selected_50hz = true; nxtEraseRect(55, 34, 80, 16); nxtDisplayCenteredBigTextLine(4, "50 60"); nxtDrawRect(19, 34, 44, 16); } break; // if the right button is pressed, set the sensor for 60Hz case kRightButton: if (!selected_50hz) { PlaySound(soundBlip); while(bSoundActive) {wait1Msec(1);} } else { selected_50hz = false; nxtEraseRect(19, 34, 44, 16); nxtDisplayCenteredBigTextLine(4, "50 60"); nxtDrawRect(55, 34, 80, 16); } break; // Make the setting permanent by saving it to the sensor case kEnterButton: eraseDisplay(); nxtDisplayCenteredTextLine(2, "The Sensor is"); nxtDisplayCenteredTextLine(3, "configured for"); if (selected_50hz) { _HTCSsendCommand(HTCS2, 0x35); nxtDisplayCenteredTextLine(4, "50 Hz operating"); } else { _HTCSsendCommand(HTCS2, 0x36); nxtDisplayCenteredTextLine(4, "60 Hz operating"); } nxtDisplayCenteredTextLine(5, "frequency"); for (int i = 5; i > 0; i--) { nxtDisplayCenteredTextLine(7, "Exiting in %d sec", i); wait1Msec(1000); } StopAllTasks(); break; } // Debounce the button while (nNxtButtonPressed != kNoButton) { wait1Msec(1); } } }
// update motor speed and wait for duration void control(int left, int right, int duration = 0){ motor[Left] = left; motor[Right] = right; if(duration) wait1Msec(duration); }
/*-----------------------------------------------------------------------------*/ void iqMenuConfiguration() { static tConfigSelect configSelect = kConfig_exit; static int offset = 0; bool done = false; bool update = true; // Wait for LCD button release while( nLCDButtons != kButtonNone ) wait1Msec(10); eraseDisplay(); while( !done ) { // Display menu if( update ) { iqMenuConfigurationDisplay( configSelect, offset ); update = false; } // Check LCD buttons if( nLCDButtons != kButtonNone ) { if( nLCDButtons == kButtonDown ) { if( configSelect++ == kConfig_6 ) configSelect = kConfig_6; if( configSelect > kConfig_3 && offset == 0 ) offset = 3; } if( nLCDButtons == kButtonUp ) { if( configSelect-- == kConfig_exit ) configSelect = kConfig_exit; if( configSelect < kConfig_2 && offset == 3 ) offset = 0; } if( nLCDButtons == kButtonSelect ) { if( iqMenuConfigurationSelect( configSelect ) ) done = true; } // Exit doesn't really work if( nLCDButtons == kButtonExit ) { done = true; } // Wait for LCD button release while( nLCDButtons != kButtonNone ) wait1Msec(10); // update display update = true; } wait1Msec(25); } // erase before we leave this sub menu eraseDisplay(); }