task Arm1() { int armPower; int armDirection; int armPower_raw; int limit = 75; int cut = 2.5; float Gain2 = 0.07; float Gain3 = 0.1; while (true) { getJoystickSettings(joystick); if (mode == 1) { armDirection = -1; } else if (mode == 2) { armDirection = 1; } else { armDirection = -1; } armPower_raw = joystick.joy1_y2; if (abs(armPower_raw) > deadband) { armPower = (armPower_raw - sgn(armPower_raw * deadband)); armPower = ((armPower * Gain2) * (abs(armPower) * Gain3)); if (armPower > limit) { armPower = limit; } else if (armPower < -limit) { armPower = -limit; } motor[Block_Arm] = (armDirection*armPower)/cut; } else { armPower = 0; motor[Block_Arm] = armPower; } if(joy1Btn( BTN_RB )) { if (ServoValue[Arm] == 0) //If the arm servo is open { servo[Arm] = 200; //Closes arm servo } if(ServoValue[Arm] == 200) { servo[Arm] = 0; // } } if(joy1Btn(BTN_RT)) { motor[Right_Side] = -40; motor[Left_Side] = 40; motor[Arm] = 60; wait10Msec(100); motor[Right_Side] = 0; motor[Left_Side] = 0; motor[Arm] = 0; } } }
task main() { waitForStart(); initializeRobot(); initialize_gyro(); StartTask(process_gyro); motor[finger] = 40; int irSensorVal; // move forward, turn 45 degrees, move forward move(1, 24, 2000); 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(1, 45, 4000); wait10Msec(100); turngyro_left(83.0, 30); wait10Msec(100); //move(1, 0.5, 2000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1600) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; break; //------------------------------------------------------------------ case 2: wait10Msec(100); move(-1, 2, 1000); turngyro_left(-45.0, 50); wait10Msec(50); ClearTimer(T1); nMotorEncoder[RightMotor] = 0; nMotorEncoder[LeftMotor] = 0; /*while(sensorValue[sonic] < 30) { if(irSensorVal < 2) { motor[RightMotor] = -30; motor[LeftMotor] = 30; } if(irSensorVal == 2) { motor[RightMotor] = 30; motor[LeftMotor] = 30; } if(irSensorVal > 2) { motor[RightMotor] = 30; motor[LeftMotor] = -30; } } */ move(1, 56, 4000); ClearTimer(T1); move(-1, 12.5, 2000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1600) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; break; //----------------------------------------------------- case 3: move(1, 43, 2000); wait10Msec(100); turngyro_left(-90.0, 50); wait10Msec(100); move(1, 2, 1000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1500) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; break; } /////////////////////////////////////////////////////////// /* if (irSensorVal == 4) { wait1Msec(500); motor[Leftmotor] = 50; motor[Rightmotor] = 50; wait1Msec(500); PlaySound(soundLowBuzz); } if (irSensorVal == 3) { motor[Leftmotor] = 30; motor[Rightmotor] = 60; PlaySound(soundFastUpwardTones) } if (irSensorVal == 5) { motor[Leftmotor] = 30; motor[Rightmotor] = 60; PlaySound(soundFastUpwardTones); } if (irSensorVal < 3) { motor[Leftmotor] = -50; motor[Rightmotor] = 50; //wait1Msec(200); PlaySound(soundFastUpwardTones); } if (irSensorVal > 5) { motor[Leftmotor] = 50; motor[Rightmotor] = -50; //wait1Msec(200); PlaySound(soundBeepBeep); } //else //{ //} wait1Msec(50); //motor[LeftMotor] = -50; //motor[RightMotor] = -50; */ }
///////////////////////////////////////////////////////////////////////////////////////////////////// // // Main Task // // The following is the main code for the autonomous robot operation. Customize as appropriate for // your specific robot. // // The types of things you might do during the autonomous phase (for the 2008-9 FTC competition) // are: // // 1. Have the robot follow a line on the game field until it reaches one of the puck storage // areas. // 2. Load pucks into the robot from the storage bin. // 3. Stop the robot and wait for autonomous phase to end. // // This simple template does nothing except play a periodic tone every few seconds. // // At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program. // ///////////////////////////////////////////////////////////////////////////////////////////////////// task main() { initializeRobot(); while(true) { bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase. eraseDisplay(); // WriteShort(hFileHandle, nIoResult, nParm); if ( externalBatteryAvg < 0) { while (externalBatteryAvg < 0) { eraseDisplay(); nxtDisplayTextLine(1, "Ext Batt: OFF"); //External battery is off or not connected nxtDisplayCenteredBigTextLine(1, "What!?"); nxtDisplayTextLine(3, "My name is,"); nxtDisplayTextLine(4, "Iego Montoya,"); nxtDisplayTextLine(5, "you did not turn"); nxtDisplayTextLine(6, "on the robot"); nxtDisplayTextLine(7, "NOW YOU DIE"); nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!"); PlayImmediateTone(600, 20); PlayImmediateTone(400, 20); wait10Msec(20); } } else { nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000); } while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7) { eraseDisplay(); nxtDisplayBigTextLine(6, "Battery"); nxtScrollText("poopheads"); nxtScrollText("You didnt change the battery you shmuts!"); nxtScrollText("the battery you"); nxtScrollText("shmuts!"); PlayImmediateTone(600, 70); PlayImmediateTone(400, 70); wait10Msec(20); } nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]); nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1)); nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]); nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]); wait10Msec(20); }//End of While //Actuation tests. motor[R_Motor] = 100; wait10Msec(30); motor[R_Motor] = 0; wait10Msec(30); motor[L_Motor] = 100; wait10Msec(30); motor[L_Motor] = 0; wait10Msec(30); motor[Flag_Raiser] = 100; wait10Msec(30); motor[Flag_Raiser] = 0; wait10Msec(20); motor[Winch] = 100; wait10Msec(30); motor[Winch] = 0; wait10Msec(30); motor[Arm] = 100; wait10Msec(30); motor[Arm] = 0; wait10Msec(30); motor[Block_Sweep] = 100; wait10Msec(30); motor[Block_Sweep] = 0; wait10Msec(30); servo[Bucket_Release] = 100; wait10Msec(30); servo[Bucket_Release] = 0; wait10Msec(30); servo[Spring_Release] = 100; wait10Msec(30); servo[Spring_Release] = 0; wait10Msec(30); servo[IRS_1] = 100; wait10Msec(30); servo[IRS_1] = 0; wait10Msec(30); }
//============================================================================= // Function: Move() // // Description: // This function will drive the robot the distance specified in "distance". // // Parameters: distance = distance in inches, Positive is forward, // Negative is backward //============================================================================= void Move( float distance, float max_speed ) { nMotorPIDSpeedCtrl[B_L]= mtrSpeedReg; nMotorPIDSpeedCtrl[B_R]= mtrSpeedReg; nMotorPIDSpeedCtrl[F_L]= mtrSpeedReg; nMotorPIDSpeedCtrl[F_R]= mtrSpeedReg; nMaxRegulatedSpeed12V = 1000; float COUNTS_PER_INCH = 103.55; int countDistance; int direction = 1; int motorSpeed = 0; int MAX_ENCODER_SPEED = 50; int cut = 3; float Sum_left; float Sum_right; float RestofDst = 0.75; // Determine direction. Set to negative for backwards if( distance < 0 ) { direction = -1; } if( distance <= 5 ) { motorSpeed = MAX_ENCODER_SPEED/cut; } // Calculate the distance to travel in encoder counts countDistance = abs(distance) * COUNTS_PER_INCH; // remove the backlash and freeplay from the motors before zeroing the encoders by commanding // a very low power for a short time motorSpeed = 5; motor[B_L] = direction * motorSpeed; motor[B_R] = direction * motorSpeed; motor[F_L] = direction * motorSpeed; motor[F_R] = direction * motorSpeed; wait1Msec(10); if ( (int)(abs(max_speed)) < MAX_ENCODER_SPEED) { MAX_ENCODER_SPEED = (int)max_speed; } // reset the motor encoders to zero nMotorEncoder[F_L] = 0; nMotorEncoder[F_R] = 0; nMotorEncoder[F_L] = 0; nMotorEncoder[F_R] = 0; // nMotorEncoderTarget[F_L] = countDistance; // nMotorEncoderTarget[F_R] = countDistance; motor[B_L] = direction * MAX_ENCODER_SPEED; motor[B_R] = direction * MAX_ENCODER_SPEED; motor[F_L] = direction * MAX_ENCODER_SPEED; motor[F_R] = direction * MAX_ENCODER_SPEED; while (abs(nMotorEncoder[F_R]) < countDistance && abs(nMotorEncoder[F_L]) < countDistance && abs(nMotorEncoder[B_L])< countDistance && abs(nMotorEncoder[B_R])< countDistance) { //while the encoder value is less than the distance it needs to reach wait10Msec(25); Sum_left = (abs(nMotorEncoder[F_L]) + abs(nMotorEncoder[B_L])); //Ads up the encodr values on the left and right to compare them Sum_right = (abs(nMotorEncoder[F_R]) + abs(nMotorEncoder[B_R])); /*if (Sum_left > Sum_right) { motor[F_R] = direction * MAX_ENCODER_SPEED + 5; motor[B_R] = direction * MAX_ENCODER_SPEED + 5; } else if (Sum_left < Sum_right) { motor[F_L] = direction * MAX_ENCODER_SPEED + 5; motor[B_L] = direction * MAX_ENCODER_SPEED + 5; }*/ if (abs(nMotorEncoder[F_R]) >= countDistance * RestofDst && abs(nMotorEncoder[F_L]) >= countDistance * RestofDst && abs(nMotorEncoder[B_L]) >= countDistance * RestofDst && abs(nMotorEncoder[B_R]) >= countDistance * RestofDst) { //If the encoder values becomes greater than a certain percentage of the total value needed to reach the desired distance. motor[B_L] = MAX_ENCODER_SPEED/cut; //the motor value will continue to be cut until the distance is reached. motor[B_R] = MAX_ENCODER_SPEED/cut; motor[F_L] = MAX_ENCODER_SPEED/cut; motor[F_R] = MAX_ENCODER_SPEED/cut; } } motor[B_L] = -direction*20; motor[B_R] = -direction*20; motor[F_L] = -direction*20; motor[F_R] = -direction*20; wait10Msec(10); motor[B_L] = 0; motor[B_R] = 0; motor[F_L] = 0; motor[F_R] = 0; // reset the back motor encoders to zero nMotorEncoder[F_L] = 0; nMotorEncoder[F_R] = 0; nMotorEncoder[B_L] = 0; nMotorEncoder[B_R] = 0; } // End of Move()
task main() { float temp; tLEGOTMPAccuracy accuracy; string strAcc; nxtDisplayCenteredTextLine(0, "LEGO"); nxtDisplayCenteredBigTextLine(1, "Temp"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); eraseDisplay(); // Setup the sensor for continuous mode LEGOTMPsetContinuous(LEGOTMP); //setting minimum accuracy accuracy = A_MIN; if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) { nxtDisplayTextLine(0, "Error setAccuracy"); wait1Msec(5000); StopAllTasks(); } //reads the current accuracy of the sensor if (!LEGOTMPreadAccuracy(LEGOTMP, accuracy)) { nxtDisplayTextLine(0, "Error readAccuracy"); wait1Msec(5000); StopAllTasks(); } accuracyToString(accuracy, strAcc); nxtDisplayTextLine(0, "Accuracy: %s", strAcc); //loop to read temp while (true) { switch(nNxtButtonPressed) { // If the left button is pressed, decrease the accuracy case kLeftButton: switch(accuracy) { case A_MIN: accuracy = A_MAX; break; case A_MEAN1: accuracy = A_MIN; break; case A_MEAN2: accuracy = A_MEAN1; break; case A_MAX: accuracy = A_MEAN2; break; } if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) { nxtDisplayTextLine(0, "Error setAccuracy"); wait1Msec(5000); StopAllTasks(); } accuracyToString(accuracy, strAcc); nxtDisplayTextLine(0, "Accuracy: %s", strAcc); // debounce the button while (nNxtButtonPressed != kNoButton) EndTimeSlice(); break; // If the right button is pressed, increase the accuracy case kRightButton: switch(accuracy) { case A_MIN: accuracy = A_MEAN1; break; case A_MEAN1: accuracy = A_MEAN2; break; case A_MEAN2: accuracy = A_MAX; break; case A_MAX: accuracy = A_MIN; break; } if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) { nxtDisplayTextLine(0, "Error setAccuracy"); wait1Msec(5000); StopAllTasks(); } accuracyToString(accuracy, strAcc); nxtDisplayTextLine(0, "Accuracy: %s", strAcc); // debounce the button while (nNxtButtonPressed != kNoButton) EndTimeSlice(); break; } if (!LEGOTMPreadTemp(LEGOTMP, temp)) { eraseDisplay(); nxtDisplayTextLine(0, "Temp reading pb"); wait10Msec(100); StopAllTasks(); } nxtDisplayCenteredBigTextLine(2, "Temp:"); switch(accuracy) { case A_MIN: nxtDisplayCenteredBigTextLine(4, "%4.1f", temp); break; case A_MEAN1: nxtDisplayCenteredBigTextLine(4, "%5.2f", temp); break; case A_MEAN2: nxtDisplayCenteredBigTextLine(4, "%6.3f", temp); break; case A_MAX: nxtDisplayCenteredBigTextLine(4, "%7.4f", temp); break; } } }
task main() { waitForStart(); moveForward(0.15, 70); wait10Msec(50); stopMotors(); wait10Msec(30); rightTwoWheelTurn(45, 40); wait10Msec(70); stopMotors(); wait10Msec(30); //robot moves backward, so it will be able to sense the ir beacon underneath first bucket /*moveBackward(0.25, 80); wait10Msec(50);*/ moveForward(6, 100); wait10Msec(50); stopMotors(); wait10Msec(30); //keep going forward until ir sensor senses ir beacon StartTask(irRightTesting); wait10Msec(1400); //wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum while (SensorValue[sonarSensor] < 80) { motor[frontLeftMotor] = 100; motor[frontRightMotor] = -100; motor[backRightMotor] = 100; motor[backLeftMotor] = 100; } stopMotors(); wait10Msec(10); moveForward(1.5, 90); wait10Msec(50); leftTwoWheelTurn(48, 85); wait10Msec(40); moveForward(30, 90); wait10Msec(50); leftTwoWheelTurn(42, 85); wait10Msec(30); moveForward(32, 90); wait10Msec(50); leftTwoWheelTurn(59, 85); wait10Msec(70); moveForward(44, 80); wait10Msec(50); //robot is parked in the middle of the ramp }
void ProgrammingSkillsAutonomous() { armPosition = SensorValue[ArmPoten]; SensorValue[Gyroscope] = 0; StartTask(KeepArmInPosition); // A: Pick up stack at intersection armPosition = stackHeight; Intake(127); wait10Msec(40); Forward(45, 35, "none"); // B: Forward on line to doubler, pick up doubler, stop at intersection ForwardOnLine(45, 50, "none"); Intake(0); ForwardOnLine(45, 100, "none"); ForwardOnLine(45, 40, "none"); /// Intake(127); armPosition = minArm + 20; Forward(55, 100, "Intersection"); StopMoving(); armPosition = minArm + 100; Backward(45, 10, "Intersection"): Backward(45, 7, "none"): StopMoving(); // C: Turn 90° left to face 30" goal, forward, score doubler + 5 objects in 30" goal TurnLeftDegrees(91); Intake(0); armPosition = maxArm + 50; wait10Msec(100); Forward(50, 33.5, "none"); StopMoving(); Intake(-127); wait10Msec(60); Intake(-65); wait10Msec(60); Intake(-120); wait10Msec(40); Intake(-65); wait10Msec(60); Intake(-120); wait10Msec(80); // Intake(-127); // wait10Msec(250); Intake(0); // D: Back up, lower arm, forward and pick up stack in front of 30" goal //start of what grace programmed Backward(50, 50, "Intersection"); wait10Msec(5); StopMoving(); wait10Msec(20); armPosition = stackHeight + 20; wait10Msec(90); Intake(127); Forward(35, 28, "none"): StopMoving(); // E: Back up, turn 90° right to face 11.5" goal, score 11.5" goal Backward(35, 40, "Intersection"); Backward(35, 8, "none"); StopMoving(); TurnRightDegrees(90); Intake(0); armPosition = lowGoalHeight; wait10Msec(20); Forward(35, 16, "none"); StopMoving(); wait10Msec(10); Intake(-85); wait10Msec(50); Intake(-0); wait10Msec(20); Intake(-85); wait10Msec(70); Intake(0); // F: Back up to intersection, turn 90° right, forward a lot, pick up stack, score stack on 11.5" goal Backward(85, 30, "none"); armPosition = stackHeight; // Backward(85, 50, "none"); // Backward(65, 35, "none"); // Backward(45, 40, "none"); Backward(85, 120, "none"); Backward(35, 70, "Intersection"); wait10Msec(1); StopMoving(); TurnRightDegrees(90); ForwardOnLine(35, 20, "none"); ForwardOnLine(45, 80, "none"); Intake(127); ForwardOnLine(45, 53, "none"); // armPosition = stackHeight;//// Forward(45, 40, "Intersection"); StopMoving(); armPosition = lowGoalHeight + 40; wait10Msec(40); Forward(45, 25, "none"); StopMoving(); Intake(-100); wait10Msec(55); Intake(-0); wait10Msec(20); Intake(-85); wait10Msec(55); Intake(0); // G: Back up, pick up stack in front of 11.5" goal, back up to intersection, turn 90° left Backward(55, 28, "none"); StopMoving(); armPosition = stackHeight + 20; Intake(127); Forward(45, 40, "none"); StopMoving(); Backward(55, 60, "Intersection"); Backward(45, 5, "none"); StopMoving(); TurnLeftDegrees(90); Backward(85, 40, "none"); StopMoving(); Intake(0); //======================================== PART 2 =============================================================== while (SensorValue[Bump] == 0) {} // A2: Go forward SensorValue[Gyroscope] = 0; Forward(50, 30, "none"); // B2: Forward on line to doubler, pick up doubler, stop at intersection ForwardOnLine(45, 130, "none"); // ForwardOnLine(45, 20, "none");/// Intake(127); armPosition = minArm + 40; Forward(55, 100, "Intersection"); StopMoving(); armPosition = minArm + 120; Backward(45, 10, "Intersection"): Backward(45, 5, "none"): StopMoving(); // C2: Turn 90° right to face 30" goal, forward, score doubler + 5 objects in 30" goal Intake(0); TurnRightDegrees(91); // Intake(0); StartTask(KeepArmInPosition); armPosition = maxArm + 50; wait10Msec(90); Forward(40, 40.5, "none"); StopMoving(); Intake(-100); wait10Msec(60); Intake(-55); wait10Msec(90); Intake(-100); wait10Msec(50); Intake(-55); wait10Msec(60); Intake(0); // D2: Back up, lower arm, forward and pick up stack in front of 30" goal Backward(55, 50, "Intersection"); StopMoving(); wait10Msec(20); armPosition = stackHeight; wait10Msec(100); Intake(127); Forward(45, 22, "none"): StopMoving(); // E2: Back up, turn 180° left to face 20" goal, forward and pick up 2 objects Backward(55, 50, "Intersection"); StopMoving(); TurnLeftDegrees(186); Intake(0); armPosition = stackHeight + 180; wait10Msec(10); Intake(127); Forward(55, 14, "none"); StopMoving(); wait10Msec(5); armPosition = stackHeight + 300; Backward(55, 6, "none"); armPosition = minArm + 60; wait10Msec(5); Forward(55, 30, "none"); Backward(55, 20, "none"); StopMoving(); //F2: Raise arm to midgoal height, score 2 and 2 on 20" goals armPosition = midGoalHeight + 40; wait10Msec(100); Intake(0); Forward(55, 13, "none"); StopMoving(); wait10Msec(10); Intake(-95); wait10Msec(300); /* wait10Msec(80); Intake(100); Forward(55, 27, "none"); StopMoving(); Intake(-95); wait10Msec(160); */ StopTask(KeepArmInPosition); }
task main() { while(SensorValue[bumperLeft] == 0) { } StartTask(intakeStart); //StartTask(armcontrol); //armPos = 1000; // Start from hanging zone (Top left) ClearTimer(T4); moveSecondTierUp(127,450); moveSecondTierDown(127,0); intake = 1; motor[secondTier]=-127; wait10Msec(50); motor[secondTier]=0; moveStraightDistance(127,200); stopPid(0.6,0.3); wait10Msec(10); moveStraightDistance(30, 200); stopPid(0.6,0.3); wait10Msec(50); turnRight(127,600); moveFirstTierUp(127,300); motor[firstTierLeft]=20; motor[firstTierRight]=20; intake = 0; moveStraightTime(127, 2000); moveFirstTierDown(127, 0); // Continue from middle zone (Left) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127,350); alignFoward(127); stopPid(0.6,0.3); moveStraightDistance(100,50); stopPid(0.6,0.3); wait10Msec(10); turnRight(100,280); stopTurn(0.6,0.3); moveStraightDistance(100,1400); stopPid(0.6,0.3); moveFirstTierUp(127,1200); motor[firstTierLeft] = 10; motor[firstTierRight] = 10; moveStraightDistance(100,200); turnLeft(50,30); stopTurn(0.6,0.3); moveStraightTime(50,500); intake = -1; wait10Msec(300); intake = 0; moveStraightDistance(-50,250); moveFirstTierDown(127,50); turnLeft(127,40); moveStraightDistance(-100, 1300); // Continue from Middle Zone (Left) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127, 1500); alignFoward(127); intake = 1; moveStraightDistance(40, 500); wait10Msec(50); intake = 0; // Continue from Middle Zone (Right) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(-127,300); moveFirstTierUp(127,400); motor[firstTierLeft]=20; motor[firstTierRight]=20; moveStraightDistance(127,1800); moveFirstTierDown(127,400); // Continue from Hanging Zone (Top right) while (SensorValue[bumperLeft] == 0) { } intake = 1; moveStraightDistance(127,200); stopPid(0.6,0.3); wait10Msec(10); moveStraightDistance(30, 200); stopPid(0.6,0.3); wait10Msec(170); intake = 0; turnLeft(127,650); intake = 1; moveFirstTierUp(127,400); motor[firstTierLeft]=10; motor[firstTierRight]=10; intake = 0; moveStraightDistance(127, 1300); moveFirstTierUp(127, 500); motor[firstTierLeft]=10; motor[firstTierRight]=10; // Continue from Middle Zone (Right) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127,350); stopPid(0.6,0.3); alignFoward(127); moveStraightDistance(127,100); stopPid(0.6,0.3); wait10Msec(10); turnLeft(127,380); moveStraightDistance(127,1300); stopPid(0.6,0.3); moveFirstTierUp(127,1100); moveStraightDistance(100,100); motor[firstTierLeft] = 10; motor[firstTierRight] = 10; turnRight(50,30); moveStraightDistance(50,200); intake = -1; wait10Msec(300); intake = 0; moveStraightDistance(-50,250); moveFirstTierDown(127,50); turnRight(127,50); moveStraightDistance(-50, 1300); // Continue from Middle Zone (Right) while (SensorValue[bumperLeft] == 0) { } moveFirstTierUp(127,1100); motor[firstTierLeft] = 20; motor[firstTierRight] = 20; moveStraightDistance(127,800); moveStraightDistance(-127,600); // Continue from Middle Zone (Right) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127,1200); moveStraightDistance(-127,500); turnRight(127,350); moveStraightDistance(127,1500); // Continue from Middle Zone (Left) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127,800); moveStraightDistance(-127,600); // Continue from Middle Zone (Left) while (SensorValue[bumperLeft] == 0) { } moveStraightDistance(127,1000); moveStraightDistance(-127,1100); }
task main() { accelerate(50); wait10Msec(100); }
task main() { while(SensorValue[irsensor] != 7) { DriveMotorsForward(); } wait10Msec(15); StopDriveMotors(); wait10Msec(10); TurnRight(); wait10Msec(83); StopDriveMotors(); wait10Msec(5); if(SensorValue(ultrasonic) < 30) { while(SensorValue(ultrasonic) < 30) { DriveMotorsBackward(); } } StopDriveMotors(); motor[liftassembly1] = 100; wait10Msec(600); motor[liftassembly1] = 0; if(SensorValue(ultrasonic) > 25) { while(SensorValue(ultrasonic) > 25) { DriveMotorsForward(); } } StopDriveMotors(); motor[bucketwrist] = -100; wait10Msec(500); motor[bucketwrist] = 0; DriveMotorsBackward(); wait10Msec(25); StopDriveMotors(); wait10Msec(5); motor[liftassembly1] = -100; wait10Msec(325); motor[liftassembly1] = 0; wait10Msec(30); TurnRight(); wait1Msec(1000); StopDriveMotors(); wait10Msec(30); while(SensorValue(ultrasonic) > 40) { DriveMotorsForward(); } StopDriveMotors(); wait10Msec(30); TurnLeft(); //Turn to be perpendicular to ramp wait10Msec(75); StopDriveMotors(); wait10Msec(30); DriveMotorsForward(); // Drive alongside ramp wait10Msec(200); StopDriveMotors(); wait10Msec(30); TurnLeft(); // turn to face ramp wait10Msec(107); StopDriveMotors(); wait10Msec(30); DriveMotorsForward(); //Drive onto ramp (hopefully) wait10Msec(200); StopDriveMotors(); }
task main() { waitForStart(); initializeRobot(); initialize_gyro(); StartTask(process_gyro); int irSensorVal; move(1, 24, 2000); wait10Msec(50); move(1, 0, 1000); ClearTimer(T1); wait10Msec(50); irSensorVal = SensorValue[IR]; nxtDisplayBigTextLine(2, "IR: %d", irSensorVal); ////////////////////////////////////////////////////////////////// switch(irSensorVal) { case 1: turngyro_left(-90.0, 35); wait10Msec(100); move(1, 45, 4000); wait10Msec(100); turngyro_left(83.0, 30); wait10Msec(100); move(1, 10, 1000); motor[RightMotor] = 50; wait10Msec(400); move(-1, 12, 2000); while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/) { motor[lifter] = 30; } move(1, 2, 300); while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/) { motor[lifter] = -30; } break; //------------------------------------------------------------------ case 2: wait10Msec(100); move(-1, 2, 1000); turngyro_left(-50.0, 50); wait10Msec(50); while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/) { motor[lifter] = 30; } move(1, 2, 300); while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/) { motor[lifter] = -30; } /* ClearTimer(T1); nMotorEncoder[RightMotor] = 0; nMotorEncoder[LeftMotor] = 0; move(1, 56, 4000); ClearTimer(T1); move(-1, 12.5, 2000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1600) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; while(nMotorEncoder[arm] > 800) { motor[arm] = -50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = 0;*/ break; //----------------------------------------------------- case 3: move(1, 43, 2000); wait10Msec(100); turngyro_left(-100.0, 50); wait10Msec(100); move(1, 10, 1000); motor[LeftMotor] = 50; wait10Msec(400); move(-1, 12, 2000); while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/) { motor[lifter] = 30; } move(1, 2, 300); while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/) { motor[lifter] = -30; } /* nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1500) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; while(nMotorEncoder[arm] > 800) { motor[arm] = -50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = 0;*/ break; } }
void chill(){ motor[rightWheel] = 0; motor[leftWheel] = 0; wait10Msec(100); }
void Right90deg(){ motor[rightWheel] = TurnSpeed*-1; motor[leftWheel] = TurnSpeed; angle = (angle-90 )% 360; wait10Msec(TurnTime); }
///////////////////////////////////////////////////////////////////////////////////////////////////// // // findBeacon // // This routine uses the IR seeker sensor to orient the robot based on the IR beacon // ///////////////////////////////////////////////////////////////////////////////////////////////////// int findBeacon(int iPos, string iSide) { int SensorVal1; int BeaconPos; //First we must turn the robot until the beacon is in front of us if (strcmp(iSide, "left")== 0) { nxtDisplayCenteredTextLine(5, "Left"); while (SensorValue[irSeeker] < 7 && ServoValue[IR] < 255) { servo[IR] = ServoValue[IR] + 1; } } else { nxtDisplayCenteredTextLine(5, "Right"); while (SensorValue[irSeeker] > 3 && ServoValue[IR] > 0) { servo[IR] = ServoValue[IR] - 1; } } SensorVal1 = ServoValue[IR]; BeaconPos = ServoValue[IR]; if (debugMode == ON) { eraseDisplay(); nxtDisplayCenteredTextLine(1, "Beacon Found"); nxtDisplayTextLine(3, "%i",BeaconPos); wait10Msec (10000); } if (iPos == 1) { if (strcmp(iSide, "left")== 0) { if (BeaconPos < 78) { return 1; } else if (BeaconPos < 97) { return 2; } else { return 3; } } else { if (BeaconPos < 94) { return 3; } else if (BeaconPos < 114) { return 2; } else { return 1; } } } else if (iPos == 2) { if (strcmp(iSide, "left")== 0) { if (BeaconPos < 91) { return 1; } else if (BeaconPos < 114) { return 2; } else { return 3; } } else { if (BeaconPos < 78) { return 3; } else if (BeaconPos < 103) { return 2; } else { return 1; } } } else { return 1; } }
void deploy() { intake(-1); wait10Msec(70); intake(0); }
task main() { waitForStart(); wait1Msec(1450); MotorEncoderTarget(2, 11, 11, 65, 10); if (SensorValue[IRSeeker] > 2) { MotorEncoderTarget(2, 20, 20, 65, 10); Temp1++; if (SensorValue[IRSeeker] > 2) { MotorEncoderTarget(2, 21, 21, 65, 10); Temp1++; } } wait10Msec(20); MotorEncoderTarget(1, -2100, 2100, 60, 30); //MotorEncoderTarget(1, (-2000 + Temp1*50), (1950 - Temp1*50), 60, 30); if(Temp1 == 1) { MoveLift(1, 3000, 30); } else { MoveLift(1, 600, 30); } MotorEncoderTarget(2, 30, 30, 70, 30); LineCheck(7 * 115); motor[motorL] = 0; motor[motorR] = 0; if(Temp1 == 1) { MoveLift(2, 1200, 30); } else { MoveLift(2, 550, 30); } MotorEncoderTarget(2, -36, -36, 80, 10); MotorEncoderTarget(1, 2100, -2100, 80, 10); switch (Temp1) { case 0: MotorEncoderTarget(2, -10, -10, 80, 10); break; case 1: MotorEncoderTarget(2, -30, -30, 80, 10); break; case 2: MotorEncoderTarget(2, -55, -55, 80, 10); break; } }
void progSkills() { //while (SensorValue[bumper] == 0) {} SensorValue[rightBackS] = 0; while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90); baseStop(); // Deploy deploy(); moveLiftAuto(1,3100); moveLift(20); SensorValue[rightBackS] = 0; while (abs(SensorValue[rightBackS]) < 40) moveStraight(40); baseStop(); /* moveStraight(127); wait1Msec(200); moveStraight(-127); wait1Msec(200); */ moveStraight(127); wait1Msec(200); intake(-1); moveStraight( -127); wait1Msec(200); baseStop(); //intake(-1); wait1Msec(2000); moveStraight(50); wait1Msec(100); baseStop(); intake(1); moveLift(-30); wait1Msec(500); intake(0); liftStop(); moveStraight(-40); wait1Msec(200); baseStop(); SensorValue[leftBackS] = 0; SensorValue[rightBackS] = 0; while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450) { while (SensorValue[rightLiftS] < 4000) { moveLift(-70); turnOnSpot(2,90); } liftStop(); } baseStop(); intake(1); moveStraight(70); wait1Msec(1000); baseStop(); SensorValue[leftBackS] = 0; SensorValue[rightBackS] = 0; while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500) { turnOnSpot(2,90); } baseStop(); moveStraight(127); wait1Msec(2000); baseStop(); while(SensorValue[bumper] == 0) {} intake(-1); while(SensorValue[bumper] == 0) {} intake(0); // Raise Lift // Output while(SensorValue[bumper] == 0) {} intake (1); moveStraight(127); wait10Msec(50); turnOnSpot(1,60); wait10Msec(30); turnOnSpot(2,60); wait10Msec(30); baseStop(); SensorValue[leftBackS] = 0; SensorValue[rightBackS] = 0; while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500) { turnOnSpot(1,90); } intake(-1); wait10Msec(100); }
task main() { { initializeRobot(); bFloatDuringInactiveMotorPWM = false; nxtDisplayTextLine(2, "Waiting for start"); waitForStart(); // wait for start of tele-op phase nxtDisplayTextLine(2, "starting"); StartTask( drive ); StartTask( Arm ); float watchedmessage; //int watch = 1; while(true) { watchedmessage = ntotalMessageCount; // grabs the number of Messages and names it watchedMessage wait10Msec(25); // give the Messagecount time to change it changes every 50ms if(watchedmessage == ntotalMessageCount) // checks to see if the Message change if has not it is Disconnected { // if Disconnected it runs this motor[mtr_S1_C1_1] = 0; // stops motor motor[mtr_S1_C1_2] = 0; motor[mtr_S1_C2_1] = 0; motor[mtr_S1_C2_2] = 0; motor[mtr_S1_C3_1] = 0; motor[mtr_S1_C3_2] = 0; /*motor[mtr_S1_C4_1] = 0; motor[mtr_S1_C4_2] = 0;*/ StopTask(Arm); //stops task robot control task StopTask(drive); wait1Msec(10); //gives time to spot StartTask(Arm); //restart task incase of reconnection StartTask(drive); nxtDisplayTextLine(2, "DISCONNECTED"); //displays DISCONNECTED to tell you if you are Disconnected } else//if the Message change you are connected { nxtDisplayTextLine(2, "CONNECTED");//displays CONNECTED to tell you if everything is working } } } while (true) { getJoystickSettings(joystick); if (joy1Btn(BTN_RB)) if (servo[srelease] < 100) { servo[srelease] = 245; wait10Msec(50); } else if (servo[srelease] > 100 /*|| joy1Btn(BTN_X)*/) { servo[srelease] = 0; wait10Msec(50); } if (joy1Btn(BTN_RT)) if (servo[sdunk] < 100) { servo[sdunk] = 245; wait10Msec(50); } else if (servo[sdunk] > 100 /*|| joy1Btn(BTN_X)*/) { servo[sdunk] = 0; wait10Msec(50); } if (joy1Btn(BTN_X)) { // servo[srelease] = 255; motor[belt] = -100; } else if (joy1Btn(BTN_LB)) { // servo[srelease] = 255; motor[belt] = 100; } else if (!joy1Btn(BTN_LB) && !joy1Btn(BTN_X)) motor[belt] = 0; if (joy1Btn(BTN_LT)) { if (servo[stilt] == 0) { servo[stilt] = 35; wait10Msec(50); } else if (servo[stilt] == 35) { servo[stilt] = 0; wait10Msec(50); } } } }
task usercontrol() { // StartTask(KeepArmInPosition); armPosition = SensorValue[ArmPoten]; while (true) { // #########################################- Arm and Intake -######################################################## // if (!armPositionTaskActive) LiftArm(127 * vexRT[Btn5U] - 127 * vexRT[Btn5D]); //Main joystick Control // Intake(127 * vexRT[Btn6U] - 127 * vexRT[Btn6D]); // if (!armPositionTaskActive) LiftArm(vexRT[Ch3Xmtr2]); //Partner joystick Control // Intake(vexRT[Ch2Xmtr2]); if (!armPositionTaskActive) LiftArm(vexRT[Ch3Xmtr2] + 127 * vexRT[Btn5U] - 127 * vexRT[Btn5D]); //Dual Control Intake(vexRT[Ch2Xmtr2] + 127 * vexRT[Btn6U] - 127 * vexRT[Btn6D]); // ############################################- Drive -############################################################## motor[FrontLeft] = abs(vexRT[Ch3]+vexRT[Ch1]) > 10 ? vexRT[Ch3]+vexRT[Ch1] : 0; motor[BackLeft] = abs(vexRT[Ch3]+vexRT[Ch1]) > 10 ? vexRT[Ch3]+vexRT[Ch1] : 0; motor[FrontRight] = abs(-vexRT[Ch3]+vexRT[Ch1]) > 10 ? -vexRT[Ch3]+vexRT[Ch1] : 0; motor[BackRight] = abs(-vexRT[Ch3]+vexRT[Ch1]) > 10 ? -vexRT[Ch3]+vexRT[Ch1] : 0; // ##########################################- Arm Presets -########################################################### if (vexRT[Btn5DXmtr2] == 1) MoveArmToPreset(stackHeight); if (vexRT[Btn5UXmtr2] == 1) MoveArmToPreset(lowGoalHeight); if (vexRT[Btn6UXmtr2] == 1) MoveArmToPreset(midGoalHeight); if (vexRT[Btn6DXmtr2] == 1) MoveArmToPreset(maxArm); if (vexRT[Btn8U] == 1) armPosition = maxArm; if (vexRT[Btn8L] == 1) armPosition = midGoalHeight; if (vexRT[Btn8R] == 1) armPosition = lowGoalHeight; if (vexRT[Btn8D] == 1) armPosition = minArm; if (abs(vexRT[Ch3Xmtr2]) > 20) // || time10(T2) > armPresetTimer) { StopTask(KeepArmInPosition); armPositionTaskActive = false; } if (vexRT[Btn7U] == 1) ProgrammingSkillsAutonomous(); if (vexRT[Btn7D] == 1) { Forward(127, 40, "none"); VariableMove(127, 20, 70); Forward(127, 120, "none"); StopMoving(); StartTask(StayInPosition); wait10Msec(1500); StopTask(StayInPosition); } if (vexRT[Btn7R] == 1) {TurnLeftDegrees(90); wait10Msec(50); } if (vexRT[Btn8L] == 1) {TurnRightDegrees(90); wait10Msec(50); } } }
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; } }
task main() { initializeRobot(); StartTask(Drive); waitForStart(); // wait for start of tele-op phase servo[srelease] = 140; servo[sgrab] = 100; while (true) { getJoystickSettings(joystick); /////////////////////////////////// // Release Mechanism // /////////////////////////////////// if (joy1Btn(BTN_RT)) { if (servo[srelease] < 170) { servo[srelease] = 255; wait10Msec(50); } else if (servo[srelease] > 170) { servo[srelease] = 140; wait10Msec(50); } } /////////////////////////////////// // Goal Grabber // /////////////////////////////////// if (joy1Btn(BTN_LT)) { if (servo[sgrab] < 50) { servo[sgrab] = 100; wait10Msec(50); } else if (servo[sgrab] > 50) { servo[sgrab] = 40; wait10Msec(50); } } /////////////////////////////////// // Conveyor Belt // /////////////////////////////////// if (joy1Btn(BTN_X)) { motor[belt] = -100; } else if (joy1Btn(BTN_LB)) { motor[belt] = 100; } else if (!joy1Btn(BTN_LB) && !joy1Btn(BTN_X)) motor[belt] = 0; } }
//************************ // Sobe a Rampa //************************ void F_STATE_SOBE() { PlayTone(500,10); MTASK_SET_RUN(MT_ACCEL,MT_STOP_BUTTON); int USminDist = 999, USdist,USdelta,Lado1,Lado2; float Error = 0, vI = 0, vP = 0, vD = 0, kI = 0.3, kP = 1.5, kD = 1, lD=4; tByteArray USwall; MV_StopMotors(); GARRA_V(PORT_ARD,GARRA_V_DESCE); wait10Msec(30); GARRA_V(PORT_ARD,GARRA_V_PARA); time1[T1]=0; while(ESTADO_IS_CURRENT()) { //===================================== // Le arduino para pingar os ultrasons wait10Msec(3); ReadAllUS_short(PORT_ARD,USwall); // Filtra dados //// Seta as variaveis locais como locais //// para nao dar bug (alterar o valor na outra task) Lado1=USwall[0]; Lado2=USwall[2]; //// Acha a menor distancia que o robo fica das paredes //if(Lado1+Lado2<USminDist)USminDist=USdist; //// Filtra paredes (existe ou nao) if(Lado1>60)Lado1=60-Lado2; if(Lado2>60)Lado2=60-Lado1; // Calcula o Error vD = time1[T1] * ((Lado1 - Lado2) - vD) / 1000; lD = vD; vP = (Lado1 - Lado2); vI += time1[T1] * vP / 1000; //Aplica constantes vP *= kP; vI *= kI; vD *= kD; if(vI>20) vI = 20; if(vI<-20) vI = -20; Error = vP+ vI- vD; time1[T1]=0; // Aplica erro aos motores motor[MA]=85 - Error; motor[MC]=85 + Error; //Mostra no display os valores P, I, D e Stering final (Error) eraseDisplay(); nxtDisplayString(2,"P = %i", (int)vP); nxtDisplayString(3,"I = %i", (int)vI); nxtDisplayString(4,"D = %i", (int)vD); nxtDisplayBigStringAt((46 - Error)/2, 24, "|"); //Se saiu da rampa com acceleracao, entra na sala if(!ACCEL_Rampa)ESTADO_SET_TARGET(ST_ENTRA); //===================================== } }
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 // move forward, turn 45 degrees, move forward move(1, 24, 2000); 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(1, 45, 4000); wait10Msec(100); turngyro_left(83.0, 30); wait10Msec(100); //move(1, 0.5, 2000); wait1Msec(50); break; //------------------------------------------------------------------ case 2: wait10Msec(100); move(-1, 2, 1000); turngyro_left(-45.0, 50); wait10Msec(50); ClearTimer(T1); nMotorEncoder[RightMotor] = 0; nMotorEncoder[LeftMotor] = 0; move(1, 56, 4000); ClearTimer(T1); move(-1, 12.5, 2000); wait1Msec(50); break; //----------------------------------------------------- case 3: move(1, 43, 2000); wait10Msec(100); turngyro_left(-90.0, 50); wait10Msec(100); move(1, 2, 1000); nMotorEncoder[arm] = 0; 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 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] < 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 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; } 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); 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(30-5, 5000, 90, 90); turngyro_right(135, 40); move_forward(20-2, 5000, 90, 90); turngyro_left(90, 40); move_forward(10-2, 5000, 90, 90); turngyro_left(90, 40); move_backwards(10-2, 5000, 90, 90); break; servo[colorSensorServo] = colorServoDefault; wait10Msec(200); // All Done, time to stop all tasks StopAllTasks(); } // End Main Bracket
task main() { motor[port2]=127; wait10Msec(1000); }
task autonomous() { if(sensorValue(redBlueSwitch) < SWITCH){ red = true; } else{ red = false; } if(red){ motor[hook] = -127; wait10Msec(60); motor[hook] = 64; wait10Msec(200); motor[hook] = 0; turnCW(159, 127); wait10Msec(1); move(1.3,127); wait10Msec(1); turnCCW(161, 127); wait10Msec(1); move(1.1,127); wait10Msec(1); move(.7,-127); wait10Msec(1); turnCW(125,127); wait10Msec(1); move(1.1,127); wait10Msec(1); turnCCW(125,127); wait10Msec(1); move(1.4,127); wait10Msec(1); move(.7,-127); wait10Msec(1); turnCCW(45,127); wait10Msec(1); move(1.5,127); wait10Msec(1); } else{ motor[hook] = -127; wait10Msec(60); motor[hook] = 64; wait10Msec(200); motor[hook] = 0; turnCCW(159, 127); wait10Msec(1); move(1.3,127); wait10Msec(1); turnCW(161, 127); wait10Msec(1); move(1.1,127); wait10Msec(1); move(.7,-127); wait10Msec(1); turnCCW(125,127); wait10Msec(1); move(1.1,127); wait10Msec(1); turnCW(125,127); wait10Msec(1); move(1.4,127); wait10Msec(1); move(.7,-127); wait10Msec(1); turnCW(45,127); wait10Msec(1); move(1.5,127); wait10Msec(1); } }
task main() { waitForStart(); initializeRobot(); initialize_gyro(); StartTask(process_gyro); motor[finger] = 40; int irSensorVal; int colorVal; int colorVal2; int eopdValleft; int LightSensor1, LightSensor2; // move forward, turn 45 degrees, move forward move(1, 24, 2000); 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); //start new code move(1, 9, 5000); wait10Msec(100); turngyro_left(90.0, 35); wait10Msec(100); move(1, 12, 5000); wait10Msec(100); while(LightSensor1 != true && LightSensor2 != true) { turngyro_left(-40, 15); wait10Msec(500); turngyro_left(20, 15); wait10Msec(500); move(1, 2, 5000); wait10Msec(500); turngyro_left(60, 15); wait10Msec(500); turngyro_left(-20, 15); wait10Msec(500); move(-1, 2, 5000); wait10Msec(500); } move(1, 8, 3000); wait10Msec(500); //end new code move(1, 45, 4000); wait10Msec(100); turngyro_left(83.0, 30); wait10Msec(100); //move(1, 0.5, 2000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1600) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; while(nMotorEncoder[arm] > 800) { motor[arm] = -50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = 0; break; //------------------------------------------------------------------ case 2: wait10Msec(100); move(-1, 2, 1000); turngyro_left(-50.0, 50); wait10Msec(50); ClearTimer(T1); nMotorEncoder[RightMotor] = 0; nMotorEncoder[LeftMotor] = 0; move(1, 56, 4000); ClearTimer(T1); move(-1, 12.5, 2000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1600) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; while(nMotorEncoder[arm] > 800) { motor[arm] = -50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = 0; break; //----------------------------------------------------- case 3: move(1, 43, 2000); wait10Msec(100); turngyro_left(-100.0, 50); wait10Msec(100); move(1, 2.5, 1000); nMotorEncoder[arm] = 0; wait1Msec(50); while(nMotorEncoder[arm] < 1500) { motor[arm] = 50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = -40; wait1Msec(750); motor[finger] = 0; while(nMotorEncoder[arm] > 800) { motor[arm] = -50; } motor[arm] = 0; wait1Msec(1500); motor[finger] = 0; break; } }
task main() { waitForStart(); stopMotors(); wait10Msec(500); moveForward(0.25, 80); wait10Msec(50); leftTwoWheelTurn(45, 50); wait10Msec(56); moveForward(41.5, 80); wait10Msec(50); rightTwoWheelTurn(45, 50); wait10Msec(135); //robot stopped in the third bucket from the right side of the pendulum stopMotors(); wait10Msec(100); motor[tiltingMotor] = 75; wait10Msec(98); motor[tiltingMotor] = 25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); motor[conveyorMotor] = 100; wait10Msec(200); motor[conveyorMotor] = 0; wait10Msec(50); motor[tiltingMotor] = -35; wait10Msec(105); motor[tiltingMotor] = -25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); leftTwoWheelTurn(42, 50); wait10Msec(127); moveForward(16, 80); wait10Msec(50); rightTwoWheelTurn(48, 50); wait10Msec(53.5); moveForward(25, 80); wait10Msec(50); rightTwoWheelTurn(48, 50); wait10Msec(58); moveForward(36, 80); wait10Msec(50); leftTwoWheelTurn(53, 50); wait10Msec(115); moveBackward(49, 80); wait10Msec(50); //robot parked in the middle of the ramp }
task main() { initializeRobot(); //Executes all code in the initializeRobot() function StartTask( watchdog ); //Start watchdog task which is a failsafe for the Move function waitForStart(); //Wait for the beginning of autonomous phase wait10Msec(25); //Pause for 250 miliseconds Move(1,40); //Move away from the wall 1 inch at 40% power wait10Msec(25); //Pause for 250 miliseconds motor[block_arm] = 100; //Give 100% power to the arm motor wait10Msec(66); //Allow the arm to run for 660 miliseconds motor[block_arm] = 0; //Kill the power to the arm motor wait10Msec(25); //Pause for 250 miliseconds servo[Claw] = 10; //Move block release servo to open position (10 degrees) allowing it to drop the block wait10Msec(100); //Pause for 1000 miliseconds to allow the block to fall motor[block_arm] = -100; //Give 100% power to the arm again but in the oppisite direction wait10Msec(35); //Allow the arm to move back to a closed position motor[block_arm] = 0; //Kill the power to the arm motor wait10Msec(25); //Pause for 250 miliseconds Turn(-80); //Turn 80 degrees away from the bucket wait10Msec(25); //Pause for 250 miliseconds Move(22,75); //Move 22 inches towards the ramp at 75% power wait10Msec(25); //Pause for 250 miliseconds Turn(125); //Turn 55 degrees towards the ramp wait10Msec(25); //Pause for 250 miliseconds Move(53,75); //Move onto the ramp for the end of the autonomous phase wait10Msec(25); //Pause for 250 miliseconds }
task main() { writeDebugStreamLine("initalizing"); initializeRobot(); writeDebugStreamLine("initalized"); #ifdef COMPETITION waitForStart(); #else while (nNxtButtonPressed != kEnterButton){} #endif rotateDegrees(45); while (nNxtButtonPressed != kRightButton){} rotateDegrees(45); while (nNxtButtonPressed != kRightButton){} if (TestHook == 1) { setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5); while (nNxtButtonPressed != kRightButton){} hookGrab(); wait1Msec(3000); moveInches(-20); hookUngrab(); while (nNxtButtonPressed != kLeftButton){} } if (TestHardware == 1) { writeDebugStreamLine("Test Hardware"); trapDoorOpen(); trapDoorClose(); servo[collector] = COLLECTOR_IDLE; setPosition (tas, POS_BALL_COLLECTING, DEFAULT_DISTANCE); wait10Msec(300); servo[collector] = COLLECTOR_IN; wait10Msec(300); servo[collector] = COLLECTOR_IDLE; setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5); wait10Msec(1000); trapDoorOpen(); wait10Msec(3000); } if (TestPositions == 1) { writeDebugStreamLine("Test Positions"); // while (nNxtButtonPressed != kRightButton){} //setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); // while (nNxtButtonPressed != kLeftButton){} // setPosition (tas, POS_AT_60CM, DEFAULT_DISTANCE); // while (nNxtButtonPressed != kRightButton){} //Test 90 //setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); // while (nNxtButtonPressed != kLeftButton){} // setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE); // while (nNxtButtonPressed != kRightButton){} // trapDoorOpen(); // while (nNxtButtonPressed != kEnterButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kLeftButton){} setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE); while(nNxtButtonPressed != kEnterButton){} moveInches(24); while (nNxtButtonPressed != kRightButton){} trapDoorOpen(); while(nNxtButtonPressed != kEnterButton){} moveInches(-24); while (nNxtButtonPressed != kLeftButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kEnterButton){} setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE); while (nNxtButtonPressed != kRightButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kLeftButton){} setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE+3.5); while (nNxtButtonPressed != kRightButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kLeftButton){} setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE); while (nNxtButtonPressed != kRightButton){} trapDoorOpen(); while (nNxtButtonPressed != kRightButton){} setPosition (tas, POS_AT_30CM, DEFAULT_DISTANCE); while (nNxtButtonPressed != kLeftButton){} //setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); //while (nNxtButtonPressed != kRightButton){} setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE); while (nNxtButtonPressed != kEnterButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kRightButton){} setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE); //while (nNxtButtonPressed != kLeftButton){} setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); while (nNxtButtonPressed != kEnterButton){} } #if 0 setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); wait10Msec(200); //wait 2 sec for raise //moveInches(50); moveInches(54); setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5); wait10Msec(1000); //moveInches(14); moveInches(10); trapDoorOpen(); wait10Msec(500); moveInches(-20); setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE); wait10Msec(2000); #endif }
task main() { initializeRobot(); unsigned short waitValue; while (nNxtButtonPressed != 0 && nNxtButtonPressed != 3){ nxtDisplayCenteredTextLine(0, "%d milliseconds", waitValue); if (nNxtButtonPressed == 1){ waitValue += 1000; wait1Msec(325); } if (nNxtButtonPressed == 2){ waitValue -= 1000; wait1Msec(300); } if (waitValue > 18000) waitValue = 0; if (waitValue < 0) waitValue = 18000; } nxtDisplayCenteredTextLine(0, "Wait Time Done"); waitForStart(); wait1Msec(waitValue); ClearTimer(T1); ClearTimer(T2); resetEncoders(); while (time1[T2] < firstMove){ move(-80); if (time1[T1] < 1750){ motor[Lift1] = 100; motor[Lift2] = 100; } else { motor[Lift1] = 0; motor[Lift2] = 0; } } move(0); while (time1[T1] < 1750){ motor[Lift1] = 100; motor[Lift2] = 100; } motor[Lift1] = 0; motor[Lift2] = 0; //dump cube servo[bucket] = 170; wait1Msec(2000); servo[bucket] = 135; //continue to lift resetEncoders(); ClearTimer(T1); ClearTimer(T2); while (time1[T2] < secondMove){ move(-80); if (time1[T1] < 1300){ motor[Lift1] = -100; motor[Lift2] = -100; } else { motor[Lift1] = 0; motor[Lift2] = 0; } } move(0); wait10Msec(50); resetEncoders(); ClearTimer(T2); while (time1[T2] < turnTime){ turn(-80); if (time1[T1] < 1300){ motor[Lift1] = -100; motor[Lift2] = -100; } else { motor[Lift1] = 0; motor[Lift2] = 0; } } move(0); resetEncoders(); while (time1[T2] < upRamp){ move(-80); if (time1[T1] < 1300){ motor[Lift1] = -100; motor[Lift2] = -100; } else { motor[Lift1] = 0; motor[Lift2] = 0; } } move(0); while (time1[T1] < 1300){ motor[Lift1] = -100; motor[Lift2] = -100; } motor[Lift1] = 0; motor[Lift2] = 0; }