C++ EndTimeSlice函數代碼示例

task flywheelStabilization() { //modulates motor powers to maintain constant flywheel velocity
	clearTimer(T1);
	float prevError;
	float error;
	float integral;
	int numbbup = 0; //debug
	float totalError = 0;
  int numloops = 0;

	while (true)
	{
		prevError = targetVelocity - flywheelVelocity;
		integral = 0;

		while (abs(targetVelocity - flywheelVelocity) < bangBangErrorMargin * targetVelocity && targetVelocity > 0/*true*/) { EndTimeSlice();	}

		//bang bang control
		bangBangCount += 1;
		numbbup += (targetVelocity > flywheelVelocity ? 1 : 0);
		bbpercentup = 100 * numbbup / bangBangCount;
		bangBangPerSec = (float)((float)bangBangCount * 1000) / (float)(time1(T1) + .1);
		while (abs(targetVelocity - flywheelVelocity) > bangBangErrorMargin * flywheelVelocity  * 0.75 && targetVelocity > 0) {
			setLauncherPower((targetVelocity > flywheelVelocity) ? (127) : ( 0));
			EndTimeSlice();
		}

		setLauncherPower(defaultPower);
		while (targetVelocity == 0) { EndTimeSlice(); } //pauses while
	}
}

task main () {

  // Create two new timer index numbers
  int timer1 = TMRnewTimer();
  int timer2 = TMRnewTimer();

  // Configure timer1 for 2000ms
  TMRsetup(timer1, 100);

  // Configure timer2 for 5000ms
  TMRsetup(timer2, 5000);

  // Reset and start both timers
  TMRreset(timer1);
  TMRreset(timer2);

  while (true) {
    // If timer1 expires, make a small noise and reset it.
    if (TMRisExpired(timer1)) {
      PlaySound(soundBlip);
      while(bSoundActive) EndTimeSlice();
      TMRreset(timer1);
    }

    // If timer2 expires, make a small noise and reset it.
    if (TMRisExpired(timer2)) {
      PlaySound(soundShortBlip);
      while(bSoundActive) EndTimeSlice();
      TMRreset(timer2);
    }
    EndTimeSlice();
  }
}

task autonomous() {
	//start flywheel
	initializeTasks();
	setFlywheelRange(2);

	wait1Msec(1000);
	startTask(fire);
	//wait until first set of preloads are fired
	waitUntilNotFiring(15000);
	while (firing) { EndTimeSlice(); }
	stopTask(fire);

	turn(120); //turn toward stack
	motor[feedMe] = 127;
	driveStraight(150, 1, 1, 60); //move to second stack
	turn(-30);
	driveStraight(3200, 1, 1, 60); //drive across field
	autonProgress = 1;
	turn(-83); // turn toward net
	autonProgress = 2;

	//fire remaining balls
	startTask(fire);
	while (true) { EndTimeSlice(); }
}

void CEvBlk::Stop(double time)
  {
  if (pTS)
    EndTimeSlice();
  StartTimeSlice(time);
  pData[pTS->iLen++] = TS_STOP;
  EndTimeSlice();
  }

task main() {
  float temp;
  byte state = 0;

  nxtDisplayTextLine(0, "Dexter Industries");
  nxtDisplayCenteredBigTextLine(1, "T Probe");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);
  eraseDisplay();

  nxtDisplayTextLine(0, "Dexter Industries");
  nxtDisplayCenteredTextLine(7, "< switch scale >");
  //loop to read temp
  while (true) {
    switch(nNxtButtonPressed) {
      // If the right button is pressed, cycle through the scales
      case kRightButton:
        if (++state > 2)
          state = 0;
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;

        // If the left button is pressed, cycle through the scales in reverse
      case kLeftButton:
        if (--state < 0)
          state = 2;
        // debounce the button
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;
    }


    nxtDisplayCenteredBigTextLine(1, "Temp:");
    switch(state) {
      // if state: 0, display temp in degrees celcius
      case 0: DTMPreadTemp(DTMP, temp);
              nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
              nxtDisplayCenteredBigTextLine(5, "Celcius");
              break;

      // if state: 1, display temp in Fahrenheit
      case 1: DTMPreadTempF(DTMP, temp);
              nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
              nxtDisplayCenteredBigTextLine(5, "Fahrenh.");
              break;

      // if state: 2, display temp in Kelvin
      case 2: DTMPreadTempK(DTMP, temp);
              nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
              nxtDisplayCenteredBigTextLine(5, "Kelvin");
              break;
    }
    wait1Msec(10);
  }
}

task main()
{
	int tempAngle;
	eraseDisplay();
	wait1Msec(11);

	nxtDisplayStringAt(0, 63, "Minigolf 2013");

	nxtDisplayStringAt(0, 48, "GyroSensor:");
	nxtDisplayStringAt(0, 40, "SonarSensor:");

	//StartTask(GyroDeviceDriver);
	StartTask(DistDeviceDriver);
	StartTask(getHeading);

	while(!_GyrodriverInitFinish) // Gyro initialisierung
	{
		EndTimeSlice();
	}
	tempAngle = (int)_fGyroAngle;
	wait1Msec(4000);
	while(tempAngle != (int)_fGyroAngle)
	{
			nxtDisplayStringAt(0, 48, "GYRO FAIL, %02d",(int)_fGyroAngle);
			nxtDisplayStringAt(0, 40, "Drift after INIT");
	}

	searchObjekt();
	wait1Msec(500);
	calc_koordinaten_Ball(_ObjectAngle,_ObjectDistance+45);
	wait1Msec(500);
	calc_degree_hit();
	wait1Msec(500);
	calc_koordinaten_Drive();
	wait1Msec(500);
	turn2Degree(900);
	wait1Msec(500);
	driveDistance(_distance_X2Drive);
	wait1Msec(500);
	turn2Degree(0);
	wait1Msec(500);
	driveDistance(_distance_Y2Drive);
	wait1Msec(500);
	turn2Degree(_degree_Hit);
	wait1Msec(500);
	driveDistance(50);
	wait1Msec(500);
	schlagen(40,60);

	while(1)
	{
		EndTimeSlice();
	}
	StopAllTasks();
}

task main () {

    int magFieldValue = 0;
    int calibrationValue = 0;

    nxtDisplayCenteredTextLine(0, "HiTechnic");
    nxtDisplayCenteredBigTextLine(1, "MAGNETIC");
    nxtDisplayCenteredTextLine(3, "Field Sensor");
    nxtDisplayCenteredTextLine(4, "Test 1");
    nxtDisplayCenteredTextLine(5, "Connect Sensor");
    nxtDisplayCenteredTextLine(6, "to S1");

    wait1Msec(2000);

    nxtDisplayCenteredTextLine(5, "Press enter");
    nxtDisplayCenteredTextLine(6, "to set bias");

    wait1Msec(2000);
    eraseDisplay();
    while(true) {
        eraseDisplay();
        nxtDisplayTextLine(1, "Resetting");
        nxtDisplayTextLine(2, "bias");
        wait1Msec(500);

        // Start the calibration and display the offset
        calibrationValue = HTMAGstartCal(HTMAG);
        nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);
        PlaySound(soundBlip);
        while(bSoundActive) EndTimeSlice();
        while(nNxtButtonPressed != kNoButton) EndTimeSlice();

        while(nNxtButtonPressed != kEnterButton) {
            eraseDisplay();

            // Read the current calibration offset
            calibrationValue = HTMAGreadCal(HTMAG);

            // Read the current magnetic field strength
            magFieldValue = HTMAGreadVal(HTMAG);

            nxtDisplayTextLine(1, "Reading");
            // Display the current calibration value
            nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);

            nxtDisplayClearTextLine(4);
            // Display the current magnetic field strength
            nxtDisplayTextLine(4, "Mag:   %4d", magFieldValue);
            nxtDisplayTextLine(6, "Press enter");
            nxtDisplayTextLine(7, "to recalibrate");
            wait1Msec(100);
        }
    }
}

task main () {
  int raw = 0;
  int nrm = 0;
  // Get control over the buttons
  nNxtButtonTask  = -2;

  eraseDisplay();
  nxtDisplayTextLine(0, "Dexter Industries");
  nxtDisplayCenteredBigTextLine(1, "dFlex");
  nxtDisplayCenteredTextLine(3, "Test 2");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);

  eraseDisplay();
  nxtDisplayTextLine(0, "dFlex Calibration");
  nxtDisplayTextLine(2, "Left:  set min");
  nxtDisplayTextLine(3, "Right: set max");
  nxtDisplayTextLine(7, "Grey:  exit");

  while (true) {
    switch(nNxtButtonPressed) {
      // if the left button is pressed calibrate the black value for the sensor
      case kLeftButton:
                        DFLEXcalLow(DFLEX);
                        PlaySound(soundBeepBeep);
                        while(bSoundActive) EndTimeSlice();
                        break;

      // if the left button is pressed calibrate the white value for the sensor
      case kRightButton:
                        DFLEXcalHigh(DFLEX);
                        PlaySound(soundBeepBeep);
                        while(bSoundActive) EndTimeSlice();
                        break;
    }

    nxtDisplayClearTextLine(5);
    nxtDisplayClearTextLine(6);

    // Read the raw value of the sensor
    raw = DFLEXvalRaw(DFLEX);

    // Read the normalised value of the sensor
    nrm = DFLEXvalNorm(DFLEX);

    // Display the raw and normalised values
    nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);

    // Display the values for black and white
    nxtDisplayTextLine(6, "B: %4d W: %4d", dflexlow, dflexhigh);
    wait1Msec(50);
  }
}

//begin user input region
task lift() {
	while (vexRT[deployBtn] == 0) { EndTimeSlice(); }
	setLauncherPower(-40, -127, 0);
	wait1Msec(75);
	setLauncherPower(0);
	wait1Msec(750);

	while (true) {
		setLauncherPower(-127*vexRT[liftBtn] - 40*vexRT[deployBtn], -127, 0);
		EndTimeSlice();
	}
}

/*void waitForMovementToFinish(motorGroup *group, int timeout=DEF_WAIT_TIMEOUT) {
	waitForMovementToFinish(timeout, 1, group);
}*/
void waitForMovementToFinish(motorGroup *group, int timeout=DEF_WAIT_TIMEOUT) {	//TODO: delete this as soon as possible
	long movementTimer = resetTimer();

	while (time(movementTimer) < timeout) {	//wait for targeting to stabilize
		if (group->moving==TARGET && !errorLessThan(group, group->waitErrorMargin))
				movementTimer = resetTimer();

		EndTimeSlice();
	}

	while (group->moving!=TARGET && group->moving!=NO) EndTimeSlice();
}

void parseInput()
{
  writeDebugStreamLine("Beging parsing...");

  ubyte BytesRead[20];
  ubyte currByte[] = {0};
  ubyte prevByte[] = {0};
  ubyte conn[] = {0};
  int cid;
  string tmpString;
  int index = 0;
  while (true)
  {
    alive();
	  if (nxtGetAvailHSBytes() > 0)
	  {
      nxtReadRawHS(currByte[0], 1);
      if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
        index = 0;
        memset(rxbuffer, 0, sizeof(rxbuffer));
        wait1Msec(1);
        nxtReadRawHS(conn[0], 1);
        cid = conn[0] - 48;
        writeDebugStreamLine("Conn: %d", cid);
        while (true) {
          while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
          nxtReadRawHS(currByte[0], 1);

					if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
					  rxbuffer[index--] = 0;
					  rxbuffer[index--] = 0;
					  PlaySound(soundShortBlip);
					  while(bSoundActive) EndTimeSlice();
					  break;
					}
					prevByte[0] = currByte[0];
          rxbuffer[index++] = currByte[0];

				}
				for (int i = 0; i < ((index / 19) + 1); i++) {
					memset(BytesRead[0], 0, 20);
					memcpy(BytesRead[0], rxbuffer[i*19], 19);
					StringFromChars(tmpString, BytesRead);
					writeDebugStream(tmpString);

				}
				genResponse(cid);
      }
      prevByte[0] = currByte[0];
	  }
	}
}

task main() {
  float pressure;
  byte state = 0;

  nxtDisplayTextLine(0, "Dexter Industries");
  nxtDisplayCenteredTextLine(1, "dPressure 250");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);
  eraseDisplay();

  nxtDisplayTextLine(0, "Dexter Industries");
  nxtDisplayCenteredTextLine(7, "< switch scale >");
  //loop to read temp
  while (true) {
    switch(nNxtButtonPressed) {
      // If the right button is pressed, cycle through the scales
      case kRightButton:
        if (++state > 1)
          state = 0;
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;

        // If the left button is pressed, cycle through the scales in reverse
      case kLeftButton:
        if (--state < 0)
          state = 1;
        // debounce the button
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;
    }


    nxtDisplayCenteredBigTextLine(1, "Pressure:");
    switch(state) {
      // if state: 0, display temp in degrees celcius
      case 0: DPRESSreadPress250kPa(DPRESS, pressure);
              nxtDisplayCenteredBigTextLine(3, "%4.2f", pressure);
              nxtDisplayCenteredBigTextLine(5, "kPa");
              break;

      // if state: 1, display temp in Fahrenheit
      case 1: DPRESSreadPress250PSI(DPRESS, pressure);
              nxtDisplayCenteredBigTextLine(3, "%4.2f", pressure);
              nxtDisplayCenteredBigTextLine(5, "PSI.");
              break;
    }
    wait1Msec(10);
  }
}

void calibrate() {
    wait10Msec(20);
    while(nNxtButtonPressed != kEnterButton) {
        nxtDisplayCenteredTextLine(0, "Robonauts");
        nxtDisplayCenteredTextLine(1, "Offense");
        nxtDisplayCenteredBigTextLine(3, "Calibrate Compass");
        nxtDisplayTextLine(4, "Abs:   %4d", HTMCreadHeading(Compass));
        nxtDisplayCenteredTextLine(6, "Press Enter");
    }
    eraseDisplay();
    nxtDisplayTextLine(2, "Setting");
    nxtDisplayTextLine(3, "target");
    wait1Msec(500);
    // Set the current heading as the value for the offset to be used as the
    // new zero-point for the relative heading returned by
    // HTMCreadRelativeHeading()
    _target = HTMCsetTarget(Compass);
    PlaySound(soundBlip);
    while(bSoundActive) {
        EndTimeSlice();
    }
    while(nNxtButtonPressed != kEnterButton) {
        nxtDisplayCenteredTextLine(0, "Robonauts");
        nxtDisplayCenteredTextLine(1, "Offense");
        nxtDisplayCenteredBigTextLine(3, "START ROBOT");
        nxtDisplayCenteredTextLine(6, "Press Enter");
    }
    eraseDisplay();
}

////////////////////////////BALL COUNTER TASK
task BALLCOUNTER ()
{
	int CounterIn = 0;
	int CounterOut = 0;
	while(1)
	{
		//COUNT BALLS COMING IN
		if(SensorValue[IR1] == 0 && CounterIn == 0)
		{
			BallCounter++;
			CounterIn++;
		}
		if(SensorValue[IR1] == 1)
		{
			CounterIn = 0;
		}
		//COUNT BALLS COMING OUT
		if(SensorValue[IR4] == 0 && CounterOut == 0)
		{
			CounterOut++;
		}
		if(SensorValue[IR4] == 1 && CounterOut == 1)
		{
			if(BallCounter > 0)
			{
				BallCounter--;
			}
			CounterOut = 0;
		}
		EndTimeSlice(); //OR DELAY 20 MILLI
	}
}

task main() {
  int _chVal = 0;

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "Proto");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and HTPB to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);

  PlaySound(soundBeepBeep);
  while(bSoundActive) EndTimeSlice();

  eraseDisplay();

  while(true) {
    eraseDisplay();
    // get the value for ADC channel 0, we want a 10 bit answer
    _chVal = HTPBreadADC(HTPB, 0, 10);
    nxtDisplayTextLine(4, "A0: %d", _chVal);

    wait1Msec(10);
  }
}