Beispiel #1
0
task main()
{

	int irsearch = 1;
  int dirIR1 = 0;
  int dirIR2 = 0;
	int dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 = 0;
	int dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 = 0;


	while (irsearch==1)
		{

				dirIR1 = HTIRS2readACDir(irsensor1);
				HTIRS2readAllACStrength(irsensor1, dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 );
				//HTIRS2readEnhanced(irsensor1, dirEnh1, strEnh1);

				dirIR2 = HTIRS2readACDir(irsensor2);
				HTIRS2readAllACStrength(irsensor2, dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 );
				//HTIRS2readEnhanced(irsensor2, dirEnh2, strEnh2);

				nxtDisplayTextLine(7, "%d,%d,%d,%d,%d", dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2);
				nxtDisplayTextLine(6, "%d,%d,%d,%d,%d", dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1);

		}

}
Beispiel #2
0
task main()
{
    eraseDisplay();

    if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeeker) {
        writeDebugStreamLine("IRL is HTSMUXIRSeeker");
    }
    else if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeekerNew ) {
        writeDebugStreamLine("IRL is HTSMUXIRSeekerNew");
    }
    else {
        writeDebugStreamLine("IRL is unknown");
    }

    if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeeker) {
        writeDebugStreamLine("IRR is HTSMUXIRSeeker");
    }
    else if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeekerNew ) {
        writeDebugStreamLine("IRR is HTSMUXIRSeekerNew");
    }
    else {
        writeDebugStreamLine("IRR is unknown");
    }

    while(true)
    {
        eraseDisplay();

        byte status = HTSMUXreadStatus(S1);
        displayTextLine(1, "Status: %d", status);
        if(status & HTSMUX_STAT_BATT ==	HTSMUX_STAT_BATT) {
            displayTextLine (1, "no power to MUX");
            PlaySound(soundException);
            // PlaySound(soundBeepBeep);
        }

        int currentIRL = HTIRS2readACDir(IRL);
        int currentIRR = HTIRS2readACDir(IRR);

        int irl1, irl2, irl3, irl4, irl5 = -1;
        int irr1, irr2, irr3, irr4, irr5 = -1;
        HTIRS2readAllACStrength(IRL, irl1, irl2, irl3, irl4, irl5 );
        HTIRS2readAllACStrength(IRR, irr1, irr2, irr3, irr4, irr5 );

        displayTextLine(2, "D %d %d", currentIRL, currentIRR);
        displayTextLine(3, "0 %d %d", irl1, irr1);
        displayTextLine(4, "1 %d %d", irl2, irr2);
        displayTextLine(5, "2 %d %d", irl3, irr3);
        displayTextLine(6, "3 %d %d", irl4, irr4);
        displayTextLine(7, "4 %d %d", irl5, irr5);
    }
}
task updateIRVals() {
    while(true)
    {
    	if(!safe)
    	{
    		HTIRS2readAllACStrength(leftIRDev,leftFive[0],leftFive[1],leftFive[2],leftFive[3],leftFive[4]);
    		HTIRS2readAllACStrength(rightIRDev,rightFive[0],rightFive[1],rightFive[2],rightFive[3],rightFive[4]);
        left = HTIRS2readACDir(leftIRDev);
        right = HTIRS2readACDir(rightIRDev);
        safe = true;
      }
    }
}
// Y is our IR input, X is our dependant variable.
task main()
{
	clearDebugStream();

	while(true)
	{
		sum_y = 0; //from _x->_y
		sum_xTimesy = 0;
		irInputSum = 0;
		for (int j = 0; j<20; j++)
		{
			HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
			irInputSum += acS3;
		}
		irInputAvg = (irInputSum/20);

		for (int i = 0; i<20 ; i++)
			{
			HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
			if (abs(irInputAvg-acS3) > 20)
			{
				acS3 = irInputAvg;
			}
			sum_y += acS3; //READINGS: Fill our array with data from the middle node
			//sum_y += irInput[i]; //CALCULATE: The sum of each value in the array. (its easiest this way)
			sum_xTimesy += (xInput[i] * acS3) ;//EQUATION: Top left half
			writeDebugStream("%i,", acS3);
			writeDebugStreamLine("0");
			Sleep(10);
			}//APROOVED
		mean_xTimesy = (sum_x * sum_y)/20; //EQUATION: Top right half
		slopeLOBF = (sum_xTimesy-mean_xTimesy)/(sum_xSquared - mean_sum_xSquared); //CALCULATE: LOBF slope
		if (slopeLOBF<0)
		{
			PlayTone(1000,13);
		}
		//y_intLOBF = (sum_y/10) - (5.5 * slopeLOBF); //CALCULATE: LOBF y-intercept -> 5.5 = mean_x
		/*
		while (y_intLOBF >= y_intTH) //So long as we are above our y-intercept threshold AKA know we are close to the top of the curve
			{
			if (slopeLOBF <= slopeTH || slopeTH >=) //Wait untill our LOBF slope falls within our calibrated threshold
				{
				//Make beeping noise perhaps turn left because we have determined the IRSeekerV2 to be facing the IRBeacon
				}
			}
			*/
		writeDebugStream("0,");
		writeDebugStreamLine("%f, ", slopeLOBF);
		Sleep(50);
	}
}
Beispiel #5
0
void getcenterpos()
{

	//default is 2 because #2 routine works for positions 1 & 2
	int acS1, acS2, acS3, acS4, acS5;//irsensor values
	//	int acS1avg=0, acS2avg=0, acS3avg=0, acS4avg=0, acS5avg=0; //these were for testing the irsensor
	//	int prevacS1, prevacS2, prevacS3, prevacS4, prevacS5;			//these were for testing the irsensor
	int irthreshold=58.5;//to decide if position 3 or something else
	int irthreshold2=10;//to decide if position 2 or 1


	HTIRS2readAllACStrength(HTIRS2,  acS1, acS2, acS3, acS4, acS5);//used to get ir sensor values

	if(acS4>irthreshold)//finding position of centerpiece
	{
		centerpos=3;
	}
	else if(acS4>irthreshold2)
	{
		centerpos=2;
	}
	else
	{
		centerpos=1;
	}


}
// main task
task main ()
{
    // dc and ac directional values.
    int _dirDC = 0;
    int _dirAC = 0;
    // DC and AC values from 5 internal detectors.
    int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
    int acS1, acS2, acS3, acS4, acS5 = 0;
    // we are going to set DSP mode to 1200 Hz.
    tHTIRS2DSPMode _mode = DSP_1200;
    // attempt to set to DSP mode.
    if (HTIRS2setDSPMode(HTIRS2, _mode) == 0)
    {
        // unsuccessful at setting the mode.
        // display error message.
        eraseDisplay();
        nxtDisplayCenteredTextLine(0, "ERROR!");
        nxtDisplayCenteredTextLine(2, "Init failed!");
        nxtDisplayCenteredTextLine(3, "Connect sensor");
        nxtDisplayCenteredTextLine(4, "to Port 2.");
        // make a noise to get their attention.
        PlaySound(soundDownwardTones);
        // wait so user can read message, then leave main task.
        wait10Msec(300);
        return;
    }
    // initialize the array sTextLines.
    for (int i = 0; i < 8; ++i)
        sTextLines[i] = "";
    // display some header info at top of screen
    eraseDisplay();
    nxtDisplayTextLine(0, "  Piece Of Cake  ");
    nxtDisplayTextLine(1, "------3763-------");
    // loop continuously and read from the sensor.
    while(true)
    {
        // Read the current non modulated signal direction
        //_dirDC = HTIRS2readDCDir(HTIRS2);
        //	if (_dirDC < 0)
        //break; // I2C read error occurred
        //// read the current modulated signal direction
        _dirAC = HTIRS2readACDir(HTIRS2);
        if (_dirAC < 0)
            break; // I2C read error occurred
        //// Read the individual signal strengths of the internal sensors
        //// Do this for both unmodulated (DC) and modulated signals (AC)
        //if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
        //	break; // I2C read error occurred
        if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
            break; // I2C read error occurred
        nxtDisplayTextLine(2, "Position %d", _dirAC);
        nxtDisplayTextLine(3, "Strength0 %d", acS1);
        nxtDisplayTextLine(4, "Strength1 %d", acS2);
        nxtDisplayTextLine(5, "Strength2 %d", acS3);
        nxtDisplayTextLine(6, "Strength3 %d", acS4);
        nxtDisplayTextLine(7, "Strength4 %d", acS5);
        // wait a little before resuming.
        wait10Msec(5);
    }
}
task main()
{
	HTIRS2setDSPMode(irsensor, DSP_1200);
	HTIRS2setDSPMode(irsensor2, DSP_1200);
	while(true) {
  	int irArray[5];
  	int irArray2[5];
  	HTIRS2readAllACStrength(irsensor, irArray[0], irArray[1], irArray[2], irArray[3], irArray[4]);
  	HTIRS2readAllACStrength(irsensor2, irArray2[0], irArray2[1], irArray2[2], irArray2[3], irArray2[4]);

  	int p = (irArray[1]-irArray[2])-10;
  	writeDebugStream("(%d) ",p);
  	for (int i = p+20; i > 0;i-=2){
  		writeDebugStream("#");
  	}
	}
}
// main task
task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;
	string tmpString;

  // show the user what to do
  displayInstructions();

  eraseDisplay();
  for (int i = 0; i < 8; ++i)
    sTextLines[i] = "";

  // display the current DSP mode
  // When connected to a SMUX, the IR Seeker V2 can only be
  // used in 1200Hz mode.
  nxtDisplayTextLine(0, "      DC  1200");

  // The sensor is connected to the first port
  // of the SMUX which is connected to the NXT port S1.
  // To access that sensor, we must use msensor_S1_1.  If the sensor
  // were connected to 3rd port of the SMUX connected to the NXT port S4,
  // we would use msensor_S4_3

  while (true)
  {
    // Read the current non modulated signal direction
    _dirDC = HTIRS2readDCDir(HTIRS2);
    if (_dirDC < 0)
      break; // I2C read error occurred

    // read the current modulated signal direction
    _dirAC = HTIRS2readACDir(HTIRS2);
    if (_dirAC < 0)
      break; // I2C read error occurred

    // Read the individual signal strengths of the internal sensors
    // Do this for both unmodulated (DC) and modulated signals (AC)
    if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
      break; // I2C read error occurred
    if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
      break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);
    if (HTSMUXreadPowerStatus(HTSMUX))
      nxtDisplayTextLine(7, "Batt: bad");
    else
      nxtDisplayTextLine(7, "Batt: good");
  }
}
int getSeeker()
{
	int _dirDC = 0;
	int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;

	// set the DSP to the new mode
	if ( ! HTIRS2setDSPMode(seeker, DSP_1200))
		return -1; // Sensor initialized

	// Read the current non modulated signal direction
	_dirDC = HTIRS2readDCDir(seeker);
	if (_dirDC < 0)
		return -1; // I2C read error occurred

	// read the current modulated signal direction
	_dirAC = HTIRS2readACDir(seeker);
	if (_dirAC < 0)
		return -1; // I2C read error occurred

	// Read the individual signal strengths of the internal sensors
	// Do this for both unmodulated (DC) and modulated signals (AC)
	if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5))
		return -1; // I2C read error occurred
	if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 ))
		return -1; // I2C read error occurred
	count ++;
	writeDebugStreamLine("D %d %d", count, _dirAC);
	//writeDebugStreamLine("0 %d %d", dcS1, acS1);
	//writeDebugStreamLine("1 %d %d", dcS2, acS2);
	//writeDebugStreamLine("2 %d %d", dcS3, acS3);
	//writeDebugStreamLine("3 %d %d", dcS4, acS4);
	//writeDebugStreamLine("4 %d %d", dcS5, acS5);

	if(_dirAC == 2)// && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor
	{
		static int debugPrinted = 0;
		PlayTone(440<<2, 10);
		if (!debugPrinted){
			debugPrinted = true;
			writeDebugStreamLine("Played sound at %d degrees", gyroVal);
			//basketNumber = degreesToBasket(gyroVal);
		}
		return 1;
	}

	else if(_dirAC == 3)
		return 2;
	else if(_dirAC == 4)
		return 3;
	else
	{
		return -1*_dirAC;
	}

}
Beispiel #10
0
float strengthRight(){
	int ac1,ac2,ac3,ac4,ac5 = 0;			// reading of all sensors to compute "strength"
	float s;

	HTIRS2readAllACStrength(irRight, ac1, ac2, ac3, ac4, ac5 ); // reading the strength of all the sensors of the left beacon
	s = ac1+ac2+ac3+ac4+ac5;

	return(s);
}
Beispiel #11
0
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//																				 Main Task
//
// The following is the main code for the tele-op robot operation Customize as appropriate for
// your specific robot
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot Most tele-op programs will follow the following
// logic:
//	 1 Loop forever repeating the following actions:
//	 2 Get the latest game controller / joystick settings that have been received from the PC
//	 3 Perform appropriate actions based on the joystick + buttons settings This is usually a
//			simple action:
//			*	 Joystick values are usually directly translated into power levels for a motor or
//				 position of a servo
//			*	 Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//				 position
//	 4 Repeat the loop
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings
//
// At the end of the tele-op period the FMS will autonmatically abort (stop) execution of the program
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
bool bAtBeacon(void)
	{
		int zones[5];
		HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]);
		clearDebugStream();
		for(int i = 0; i < 5; i++)
			{
				writeDebugStreamLine("zone %d:\t%d", i, zones[i]);
			}
		return ((zones[1] > 50) && (zones[2] > 50) && (abs(zones[1] - zones[2]) < 10));
	}
task infared(){
	int _dirAC1 = 0;
  //int 1acS1, 1acS2, 1acS3, 1acS4, 1acS5 = 0;
  int _dirAC2 = 0;
  //int 2ac2S1, 2acS2, 2acS3, 2acS4, 2acS5 = 0;

	while(true){
		_dirAC1 = HTIRS2readACDir(IRS1);
    if (_dirAC1 < 0)
      break; // I2C read error occurred

    _dirAC2 = HTIRS2readACDir(IRS2);
    if (_dirAC2 < 0)
      break; // I2C read error occurred

    if (!HTIRS2readAllACStrength(IRS1, acS1a, acS2a, acS3a, acS4a, acS5a))
      break; // I2C read error occurred
    if (!HTIRS2readAllACStrength(IRS2, acS1b, acS2b, acS3b, acS4b, acS5b))
      break; // I2C read error occurred
	}
}
Beispiel #13
0
task main() {
	float r0 = getIRDir(sensorIR)-8, r1;
	if(r0 > 0) rbtArcRight(-7); else rbtArcLeft(20);
	rbtMoveFdDist(-10, 5000);

	ClearTimer(T1);	while(time1[T1] < 2000) {
		r1 = getIRDir(sensorIR)-8; int acS[5]; HTIRS2readAllACStrength(sensorIR, acS[0], acS[1], acS[2], acS[3], acS[4]);
		if(r0 > 0) {setLeftMotors(acS[4] > acS[3] ? -6 : -50); setRightMotors(acS[4] > acS[3] ? -50 : -8);}
    else       {setLeftMotors(acS[4] > acS[3] ? -6 : -90); setRightMotors(acS[4] > acS[3] ? -30 :  0);}
	} setLeftMotors(0); setRightMotors(0); int cr = (r0 > 0 ? (r1 > 0 ? 1 : 2) : (r1 > 0 ? 3 : 4));
	nxtDisplayBigTextLine(3, "%f", cr); for(;;);
}
Beispiel #14
0
void searchDirection()
{
	int _dirAC = 0;
	int trueDir = 0;
	int maxSig = 0;
	int acS1, acS2, acS3, acS4, acS5;

	zeroEncoder();

	// IR direction search algorithm
	while(_dirAC != 5)
	{
		// Read the current IR signal directions
		_dirAC = HTIRS2readACDir(IRSeeker);
		if ((_dirAC < 0))
		{
			writeDebugStreamLine("Read dir ERROR!");
			break; // I2C read error occurred
		}

		// Read the individual signal strengths of the internal sensors
		if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5))
		{
			writeDebugStreamLine("Read dir ERROR!");
			break; // I2C read error occurred
		} else {
			// find the max sig strength of all detectors
			maxSig = (acS1 > acS2) ? acS1 : acS2;
			maxSig = (maxSig > acS3) ? maxSig : acS3;
			maxSig = (maxSig > acS4) ? maxSig : acS4;
			maxSig = (maxSig > acS5) ? maxSig : acS5;
		}

		// which way to go?
		// 0 = no signal found
		// 1 = far left (~8 o'clock)
		// 5 = straight ahead (~12 o'clock)
		// 9 = far right (~4 o'clock)

		encoderCount = (nMotorEncoder[Lwheel] + nMotorEncoder[Rwheel]) / 2;

		// raw, dirty direction -> nice, workable direction! :-)
		trueDir = abs(_dirAC - 5);

		// only concerned with moving back/forward
		motor[Lwheel] = 20 + 5 * trueDir;
		motor[Rwheel] = 20 + 5 * trueDir;
		wait1Msec(20);
	}
}
Beispiel #15
0
// Move foward until the beacon is found.
void MovetoIR()
{
	int FindState = 1;
	bool FoundIt = false;
	nMotorEncoder[motorL] = 0;
	nMotorEncoder[motorR] = 0;
	wait1Msec(200);

	// Start moving.
	driveMotors(DRIVE_SPEED, DRIVE_SPEED);
	while(!FoundIt)
	{
		switch (FindState)
		{
		case 1:
			// Look for target
			// Get the direction.
			_dirAC = HTIRS2readACDir(IRseeker);
			// Make 0 straight ahead, all positive, no left or right worry.
			_dirAC = abs(_dirAC - 5);
			// Get the strength.
			nxtDisplayTextLine(1, "IR: %d", _dirAC);

			HTIRS2readAllACStrength(IRseeker, acS1, acS2, acS3, acS4, acS5);
		maxSig = (acS1 > acS2) ? acS1 : acS2;
		maxSig = (maxSig > acS3) ? maxSig : acS3;
		maxSig = (maxSig > acS4) ? maxSig : acS4;
		maxSig = (maxSig > acS5) ? maxSig : acS5;
			nxtDisplayTextLine(2, "maxSig: %d", maxSig);

			wait10Msec(2);
			if (_dirAC >= irGoal)
			{
				StopMotors();
				DistanceToIR = nMotorEncoder[motorR];
				FindState++;
			}
			break;
		case 2:
			// Look for strongest signal.
			FindState++;
			break;
		case 3:
			// Backup a little.
			FindState++;
			FoundIt = true;
			break;
		}
	}
}
// main task
task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;

	// the default DSP mode is 1200 Hz.
  tHTIRS2DSPMode _mode = DSP_1200;

  while(true)
  {
    // You can switch between the two different DSP modes by pressing the
    // orange enter button



    for (int i = 0; i < 8; ++i)
      sTextLines[i] = "";

    // display the current DSP mode
      nxtDisplayTextLine(0, "      DC  1200");

      // Read the current non modulated signal direction
      _dirDC = HTIRS2readDCDir(HTIRS2);
      if (_dirDC < 0)
        break; // I2C read error occurred

      // read the current modulated signal direction
      _dirAC = HTIRS2readACDir(HTIRS2);
      if (_dirAC < 0)
        break; // I2C read error occurred

      // Read the individual signal strengths of the internal sensors
      // Do this for both unmodulated (DC) and modulated signals (AC)
      if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
        break; // I2C read error occurred
      if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
        break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);

  }
}
Beispiel #17
0
// main task 
task main () 
{ 
 // dc and ac directional values. 
 int _dirDC = 0; 
 int _dirAC = 0; 
 
 // DC and AC values from 5 internal detectors. 
 int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; 
 int acS1, acS2, acS3, acS4, acS5 = 0; 
 
 // we are going to set DSP mode to 1200 Hz. 
 tHTIRS2DSPMode _mode = DSP_1200; 
 
 // attempt to set to DSP mode. 
 
 // initialize the array sTextLines. 
 for (int i = 0; i < 8; ++i) 
 sTextLines[i] = ""; 
 
 // display some header info at top of screen 
 eraseDisplay(); 
 nxtDisplayTextLine(0, " DC     AC"); 
 nxtDisplayTextLine(1, "-----------------"); 
 
 // loop continuously and read from the sensor. 
 while(true) 
 { 
 _dirDC = HTIRS2readDCDir(HTIRS2);
 _dirAC = HTIRS2readACDir(HTIRS2);
 HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
 HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
 string a = "D";
 string b = "0";
 string c = "1";
 string d = "2";
 string e = "3";
 string f = "4";
 displayText(2, a, _dirDC, _dirAC);
 displayText(3, b, dcS1, acS1); 
 displayText(4, c, dcS2, acS2); 
 displayText(5, d, dcS3, acS3); 
 displayText(6, e, dcS4, acS4); 
 displayText(7, f, dcS5, acS5); 
 
 // wait a little before resuming. 
 wait10Msec(5); 
 } 
} 
Beispiel #18
0
void searchStrength()
{
	int acS1, acS2, acS3, acS4, acS5;
	int acTemp[3];
	bool offTarget = true;

	// IR max strength search
	while(offTarget)
	{
		// get input
		for (int i = 0; i <= 2; ++i)
		{
			motor[Lwheel] = 15;
			motor[Rwheel] = 15;
			wait10Msec(50);
			// Read the individual signal strengths of the internal sensors
			if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5))
			{
				writeDebugStreamLine("Read dir ERROR!");
				break; // I2C read error occurred
			} else {
				nxtDisplayCenteredBigTextLine(3, "Sig=%d", acS3);
				acS3 = acTemp[i];
			}
		}

		// compare 3 IR strength readings
		if ((acTemp[2] > acTemp[1]) && (acTemp[2] > acTemp[0]))
		{
			motor[Lwheel] = 15;
			motor[Rwheel] = 15;
			wait10Msec(20);
		} else if ((acTemp[2] < acTemp[1]) && (acTemp[2] > acTemp[0])) {
			motor[Lwheel] = -13;
			motor[Rwheel] = -13;
			wait10Msec(20);
		} else {
			offTarget = false;
			stopRobot();
		}
		wait1Msec(500);
	}
}
int getSeeker()
{
	int _dirDC = 0;
	int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;

	// set the DSP to the new mode
	if ( ! HTIRS2setDSPMode(seeker, DSP_1200))
		return -1; // Sensor initialized

	// Read the current non modulated signal direction
	_dirDC = HTIRS2readDCDir(seeker);
	if (_dirDC < 0)
		return -1; // I2C read error occurred

	// read the current modulated signal direction
	_dirAC = HTIRS2readACDir(seeker);
	if (_dirAC < 0)
		return -1; // I2C read error occurred

	// Read the individual signal strengths of the internal sensors
	// Do this for both unmodulated (DC) and modulated signals (AC)
	if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5))
		return -1; // I2C read error occurred
	if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 ))
		return -1; // I2C read error occurred

	writeDebugStreamLine("D %d %d", _dirDC, _dirAC);
	writeDebugStreamLine("0 %d %d", dcS1, acS1);
	writeDebugStreamLine("1 %d %d", dcS2, acS2);
	writeDebugStreamLine("2 %d %d", dcS3, acS3);
	writeDebugStreamLine("3 %d %d", dcS4, acS4);
	writeDebugStreamLine("4 %d %d", dcS5, acS5);

	if(acS3 > 100 && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor
		return 1;

	else
		return 0;

}
Beispiel #20
0
/* Checks to see if we are next to the beacon */
bool bAtBeacon(void) {
	/* Declare variables */
	int zones[5];
	int average = 0;
	bool atBeacon = false;

	/* Read the infrared seeker into an array */
	HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]);

	/* Find the average infrared noise level */
	for(int i = 0; i < 5; i++) average += zones[i];
	average /= 5;

	/* If the critical zones are stronger than the noise, we are in range */
	if(((zones[1] - average) > 10) && ((zones[2] - average) > 10)) inRange = true;

	/* When we are in range and the critical zones are nearly equal (or we have overshot) we are at the beacon */
	if(inRange && ((zones[1] - zones[2]) > -15)) atBeacon = true;

	return atBeacon;
}
Beispiel #21
0
int findIRBeacon_v2()
{
    int acS1, acS2, acS3, acS4, acS5, _dirAC = 0;
    int max = 0;
    bool done;
    int dir;

    tHTIRS2DSPMode _mode = DSP_1200;
    HTIRS2setDSPMode(IRSeeker, _mode);

    moveBackwardOn(50);

    while (!done) {
        HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5);
        dir = HTIRS2readACDir(IRSeeker);
        if (dir == 5) {
            rangeFind();
        }
    }

    return 0;
}
Beispiel #22
0
task main()
{
  initializeRobot();

 // waitForStart(); // Wait for the beginning of autonomous phase.

  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////


  	// CONFIGURATION VARIABLES //
    int driveTime = 		3000;					//in MILLISECONDS   1000 MS = 1 second //
    int bucketDump = 		1600;
    int bucketStop = 		800;
    int L_Flip_Open = 	15;
    int L_Flip_Close = 	150;
   	int R_Flip_Close = 	85;
    int R_Flip_Open = 	230;





    ///////////////////////////////////////////////////////
    //    BEGIN AUTONOMOUS ROUTINE                       //
    ///////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////
// BEGIN IR ROUTINE
/////////////////////////////////////////////////////////////

int _dirAC = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
int maxSig = 0; // the max signal strength from the seeker.
int val = 0; // the translated directional value.
// we are going to set DSP mode to 1200 Hz.
tHTIRS2DSPMode _mode = DSP_1200;
// attempt to set to DSP mode.
if (HTIRS2setDSPMode(HTIRS2, _mode) == 0)
{
// unsuccessful at setting the mode.
// display error message.
eraseDisplay();
nxtDisplayCenteredTextLine(0, "ERROR!");
nxtDisplayCenteredTextLine(2, "Init failed!");
nxtDisplayCenteredTextLine(3, "Connect sensor");
nxtDisplayCenteredTextLine(4, "to Port 1.");
// make a noise to get their attention.
PlaySound(soundBeepBeep);
// wait so user can read message, then leave main task.
wait10Msec(300);
return;
}
eraseDisplay();
// loop continuously and read from the sensor.
while(true)
{
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
	{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read dir ERROR!");
break;
}
// Get the AC signal strength values.
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
{
// error! - write to debug stream and then break.
writeDebugStreamLine("Read sig ERROR!");
break;
} else {
// find the max signal strength of all detectors.
maxSig = (acS1 > acS2) ? acS1 : acS2;
maxSig = (maxSig > acS3) ? maxSig : acS3;
maxSig = (maxSig > acS4) ? maxSig : acS4;
maxSig = (maxSig > acS5) ? maxSig : acS5;
}
// display info
nxtDisplayCenteredBigTextLine(1, "Dir=%d", _dirAC);
nxtDisplayCenteredBigTextLine(4, "Sig=%d", maxSig);
// figure out which direction to go...
// a value of zero means the signal is not found.
// 1 corresponds to the far left (approx. 8 o'clock position).
// 5 corresponds to straight ahead.
// 9 corresponds to far right.
// first translate directional index so 0 is straight ahead.
val = _dirAC - 5;
// calculate left and right motor speeds.
motor[Left_drive] = 30 * val;
motor[Right_drive] = 30 * val;
// wait a little before resuming.
wait10Msec(2);
}

/////////////////////////////////////////////////////////////
// END IR ROUTINE
/////////////////////////////////////////////////////////////
/*

	 //  Drive forward for drivetime @ 20% power //
				  motor[Left_drive] = -20;
				  motor[Right_drive] = -20;
				  wait1Msec(driveTime);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;

		// Set Flippers to Open //

				  servo[Left_Flipper] = L_Flip_Open;
				  servo[Right_Flipper] = R_Flip_Open;
				  wait1Msec(2000);

		// Rotate clockwise for 1000ms  @ 20% power //
				  motor[Left_drive] = -20;
				  motor[Right_drive] = 20;
				  wait1Msec(2000);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;

   // Perform lift cycle //

				  nMotorEncoderTarget[bucket] = bucketDump;  // target bucket dumping position
				  motor[bucket] = -25;
				  while(nMotorRunState[bucket] != runStateIdle)  // While Motor is still running, allow to go to target posititon
						{
				  	// Do not continue.
						}
				  motor[bucket] = 0;

				  // if(SensorValue(touchBucketZero)== 0)
				  // {
				  nMotorEncoderTarget[bucket] = bucketStop;  // target bucket drive position
				  motor[bucket] = 25;
				  while(nMotorRunState[bucket] != runStateIdle)  // While Motor is still running, allow to go to target posititon
						{
				  	// Do not continue.
						}
				  motor[bucket] = 0;

	  	// Set Flippers to Close //

				  servo[Left_Flipper] = L_Flip_Close;
				  servo[Right_Flipper] = R_Flip_Close;
				  wait1Msec(1000);

		  // Rotate counterclockwise for 1000ms  @ 20% power //
				  motor[Left_drive] = -20;
				  motor[Right_drive] = 20;
				  wait1Msec(2000);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;

		// Drive forward for 1000ms = drivetime/3 @ 20% power //
				  motor[Left_drive] = 20;
				  motor[Right_drive] = 20;
				  wait1Msec(driveTime/3);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;

		// Rotate counterclockwise for 500ms  @ 20% power //
				  motor[Left_drive] = -20;
				  motor[Right_drive] = 20;
				  wait1Msec(2000);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;


	// Drive forward for 3500ms @ 20% power //
				  motor[Left_drive] = 40;
				  motor[Right_drive] = 40;
				  wait1Msec(2500);
				  motor[Left_drive] = 0;
				  motor[Right_drive] = 0;

		////////////////////////////
  	// End Autonomous Routine //
  	////////////////////////////
*/
}
task main()
{

	int highest_seen_value = 0;

	// TODO go forward here
	// wait for the cliff in IR readings, then break
	while(acS1 < cliff_drop_threshold)
	{
		HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
	}

	nxtDisplayString(3, "hit cliff");

	// start trying to find the spike
	bool going_down = false;
	while(!going_down)
	{
		HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
		int previous_ir_value[10];
		int previous_ir_value_difference[9];

		// set the new highest seen value
		if (highest_seen_value < acS3) highest_seen_value = acS3;

		for (int i = 0; i < 10; i++)
		{
			int number_of_negative_ir_differences = 0;

			// add the new data
			if (i < 10)
			{
				// we're in the middle (or start) of the stack, shift lower
				previous_ir_value[i] = previous_ir_value[i+1];
			} else
			{
				// i == 10, so we're at the top of the stack
				// populate with the new data
				previous_ir_value[i] = acS3;
			}

			// if this is negative, then we're probably going down
			previous_ir_value_difference[i] = previous_ir_value[i+1] - previous_ir_value[i];
			if (previous_ir_value_difference[i] < 0)
			{
				number_of_negative_ir_differences++;
			}
			// determine if we're going down with a threshold of 7/9 or more
			if (number_of_negative_ir_differences >= going_down_threshold) going_down = true;
		}
	}

	// now the trend is definitely going down, let's back up and find the highest value
	// TODO this will stop slightly before the top
	nxtDisplayString(1, "pull me backwards");
	// TODO go back here
	bool reached_highest_value = false;
	while(!reached_highest_value)
	{
		HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
		if (highest_seen_value + curve_top_change_threshold > acS3 &&
		    acS3 < highest_seen_value - curve_top_change_threshold)
		{
			reached_highest_value = true;
		}
	}
}
// main task
 task main ()
 {
  int last = 0;
  int stage = 1;
  int _dirDC = 0;
  int _dirAC = 0;
  int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  int acS1, acS2, acS3, acS4, acS5 = 0;

        // the default DSP mode is 1200 Hz.
  tHTIRS2DSPMode _mode = DSP_1200;

  // show the user what to do
  displayInstructions();
  while(true)
  {
    // You can switch between the two different DSP modes by pressing the
    // orange enter button

    PlaySound(soundBeepBeep);
    while(bSoundActive)
    {}
  eraseDisplay();
  nNumbCyles = 0;
  ++nInits;
  while (true)
  {
    if ((nNumbCyles & 0x04) == 0)
      nxtDisplayTextLine(0, "Initializing...");
    else
      nxtDisplayTextLine(0, "");
    nxtDisplayCenteredBigTextLine(1, "IR Seekr");

      // set the DSP to the new mode
    if (HTIRS2setDSPMode(HTIRS2, _mode))
        break; // Sensor initialized

      ++nNumbCyles;
      PlaySound(soundShortBlip);
      nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
      nxtDisplayCenteredTextLine(6, "Connect Sensor");
      nxtDisplayCenteredTextLine(7, "to Port S2");
      wait1Msec(100);
    }

    eraseDisplay();
    for (int i = 0; i < 8; ++i)
      sTextLines[i] = "";

    // display the current DSP mode
    if (_mode == DSP_1200)
      nxtDisplayTextLine(0, "      DC  1200");
    else
      nxtDisplayTextLine(0, "      DC   600");

    while (true)
    {
      ++nNumbCyles;
      if (nNxtButtonPressed == kEnterButton)
      {
        // "Enter" button has been pressed. Need to switch mode

        _mode = (_mode == DSP_1200) ?  DSP_600 : DSP_1200;
        while(nNxtButtonPressed == kEnterButton)
        {
          // Wait for "Enter" button release
        }
        break;
      }


      // Read the current non modulated signal direction
      _dirDC = HTIRS2readDCDir(HTIRS2);
      if (_dirDC < 0)
        break; // I2C read error occurred

      // read the current modulated signal direction
      _dirAC = HTIRS2readACDir(HTIRS2);
      if (_dirAC < 0)
        break; // I2C read error occurred

      // Read the individual signal strengths of the internal sensors
      // Do this for both unmodulated (DC) and modulated signals (AC)
      if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
        break; // I2C read error occurred
      if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
        break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);
      //displayText(7, "Last", dcS5, acS5);
      //nxtDisplayTextLine(7, "Last: "+last);
      /*
      if(acS3 >= 50){
        int speed = 20;
        motor[DFR] = -speed;
        motor[DFL]= -speed;
        motor[DBR] = -speed;
        motor[DBL]= -speed;
      }
      else if(acS4+acS5 >= 10){
        //Right
        int speed = 20;
        motor[DFR] = speed;
        motor[DFL]= -speed;
        motor[DBR] = speed;
        motor[DBL]= -speed;
      }
      else if(acS1+acS2 >= 10){
        //Left
        int speed = 20;
        motor[DFR] = -speed;
        motor[DFL]= speed;
        motor[DBR] = -speed;
        motor[DBL]= speed;
      }
      else{
        motor[DFR] = 0;
        motor[DFL]= 0;
        motor[DBR] = 0;
        motor[DBL]= 0;
      }
      */
      while(stage == 1){
      	int acS1, acS2, acS3, acS4, acS5 = 0;
      	HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
        if(acS1 > acS2-100&&last>10){

         motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = -20;

         last = -1;
         stage = 2;
         wait1Msec(1000);
         motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = 0;
       }
       else{
        if(last<acS1)
          last = acS1;
        int speed = 90-3*(acS2 + acS3);
        if (speed < 9) speed = 9;
        motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = speed;
      }
    }
    while(stage == 2){
	        //Right
	        //stage_done = 1;
      HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
      int speed = 20-acS3;
      if(speed < 9) speed = 9;
      motor[DFR] = -speed;
      motor[DFL]= speed;
      motor[DBR] = -speed;
      motor[DBL]= speed;

      if(acS3>last)
       last = acS3;
     if(last-acS3 > 1&&last!=0){

      motor[DFR] = 0;
      motor[DFL]= 0;
      motor[DBR] = 0;
      motor[DBL]= 0;
      stage = 3;
      wait1Msec(1000);
    }
       // nxtDisplayTextLine(0, "Last: "+last);
       // nxtDisplayTextLine(1, "acS3: "+dcS3);
    wait1Msec(500);

  }
}
}
}
//===================================================
// task to read in all sensors to workspace variables
//===================================================
task sensors()
{
	float currDir = 0.0; //prevDir = 0.0,
	long currtime,prevtime;
	LSsetActive(LEGOLS);							// set the LEGO light sensor to active mode
	//-------------------------
	// gyro
	//-------------------------
	ir_mux_status=HTSMUXreadPowerStatus(IR_MUX);				// read the sensor multiplexor status
	gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX);		// read the sensor multiplexor status
	while (ir_mux_status || gyro_mux_status)  					// check good battery power on both muxes
	{
		PlayTone(750,25);																	// if not we beep indefinitely
		wait1Msec(500);
	}
	//SMUX_good = true;
	while(calibrate != 1){};														// wait for a request to start calibrating the gyro
	wait1Msec(300);																			// short delay to ensure that user has released the button
	HTGYROstartCal(HTGYRO);															// initiate the GYRO calibration
	drift = MyHTCal(gyroCalTime*1000);
	Driver_Cal = HTGYROreadCal(HTGYRO);						// read the calculated calibration value for saving to file

	//---------------------------------------
	// write the GYRO calibration data to file for Tele-Op
	//---------------------------------------
	Delete(sFileName, nIoResult);												// delete any pre-existing file
	nFileSize = 100;																		// new file size will be 100 bytes
	OpenWrite(  hFileHandle, nIoResult, sFileName, nFileSize);	// create and open the new file
	WriteFloat( hFileHandle, nIoResult, drift);					// write the current drift value to the file
	WriteFloat( hFileHandle, nIoResult, Driver_Cal);		// write the driver calibration to the file
	Close(hFileHandle, nIoResult);											// close the file
	//---------------------------------------

	for (int i=0;i<5;i++)            // check if there is too much spread in the data
	{
		if (gyro_noise>10)						// if there is too much spread we beep 5 times to alert the drive team
		{
			gyroTrue = true;
			PlayTone (250,25);
			wait1Msec(500);
		}
	}
	calibrate = 2;										// this signifies to the main program that calibration has been completed
	prevtime = nPgmTime;
	while(true)
	{
		currtime=nPgmTime;
		rawgyro = HTGYROreadRot(HTGYRO);
		constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		prevtime = currtime;
		//wait1Msec(1);
		//---------------------------------------------------------------------
		// Read both sonar sensors and filter out non-valid echo readings (255)
		// If there is no echo the filter just retains the last good reading
		//---------------------------------------------------------------------
		sonarRaw = USreadDist(LEGOUS);								// read the rear mounted sensor
		if (sonarRaw!=255) sonarLive = sonarRaw;			// and copy valid results to workspace
			sonarRaw2 = USreadDist(LEGOUS2);							// read the side mounted sensor
		if (sonarRaw2!=255) sonarLive2 = sonarRaw2;		// and copy valid results to workspace

		//-------------------------
		// LEGO light sensor
		//-------------------------
		light_normalised = LSvalNorm(LEGOLS);				// read the LEGO light sensor

		//-------------------------
		// HiTechnic IR Sensor
		//-------------------------
		bearingAC = HTIRS2readACDir(HTIRS2);				// Read the IR bearing from the sensor
		bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334
		currDir = (float) bearingAC;								// copy into workspace -
		/*if (bearingAC == 0)													// No IR signal is being detected
		{
		currDir = prevDir;												// so retain the previous reading
		}
		else																				// otherwise read all the IR segments
		{
		{
		bearingAC = (bearingAC - 1)/2;
		if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
		{
		currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
		max(acS[bearingAC], acS[bearingAC + 1]);
		}
		}
		}
		prevDir = currDir;
		IR_Bearing=currDir-5;						// and setup the main variable for others to use
		*/
		HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]);
		HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]);
		//-----------------------------------
		// code for the peaks of IR sensor 1
		//-----------------------------------
		if (bearingAC!=0)								// we have a valid IR signal
		{
			int maximum = -1;
			int peak = 0, offset=0;
			for (int i=0;i<5;i++)	// scan array to find the peak entry
			{	if (acS[i]>maximum)
				{peak = i;
					maximum = acS[i];
				}
			}
			offset=0;
			if ((peak < 4) && (peak>0) && (acS[peak] != 0))  // we are not working with extreme value
			{
				if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak
				{
					if (acS[peak-1]>acS[peak+1])	// otherwise decide which side has higher signal
					{
						offset = -25*(1-(float)(acS[peak]-acS[peak-1])/		// calculate the bias away from the peak
						max(acS[peak], acS[peak-1]));
					}
					else
					{
						offset = 25*(1-(float)(acS[peak]-acS[peak+1])/
						max(acS[peak], acS[peak+1]));
					}
				}
			}
			IR_Bearing = (float)((peak-2)*50) + offset;		// direction is the total of the peak bias plus the adjacent bias
			// range is -100 to +100, zero is straight ahead
		}
		//-----------------------------------
		// code for the peaks of IR sensor 2
		//-----------------------------------
		if (bearingAC2!=0)								// we have a valid IR signal
		{
			int maximum = -1;
			int peak = 0, offset=0;
			for (int i=0;i<5;i++)	// scan array to find the peak entry
			{	if (acS2[i]>maximum)
				{peak = i;
					maximum = acS2[i];
				}
			}
			offset=0;
			if ((peak < 4) && (peak>0) && (acS2[peak] != 0))  // we are not working with extreme value
			{
				if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak
				{
					if (acS2[peak-1]>acS2[peak+1])	// otherwise decide which side has higher signal
					{
						offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/		// calculate the bias away from the peak
						max(acS2[peak], acS2[peak-1]));
					}
					else
					{
						offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/
						max(acS2[peak], acS2[peak+1]));
					}
				}
			}
			IR_Bearing2 = (float)((peak-2)*50) + offset;		// direction is the total of the peak bias plus the adjacent bias
			// range is -100 to +100, zero is straight ahead
		}
	}
}
task sensors()
{
	//-------------------------
	// gyro
	//-------------------------
	long currtime,prevtime;
	int acS[5];
	while (HTSMUXreadPowerStatus(S3))  // check battery power is on
	{
		PlayTone(750,25);
		wait1Msec(500);
	}
	SMUX_good = true;
	while(calibrate != 1){};
	wait1Msec(300);
	HTGYROstartCal(HTGYRO);
	float drift = MyHTCal(gyroCalTime*1000);

	for (int i=0;i<5;i++)            // check if there is too much spread in the data
	{
		if (abs(highest-lowest)>10)
		{
			PlayTone (250,25);
			wait1Msec(500);
		}
	}
	calibrate = 2;
	prevtime = nPgmTime;
	while(true)
	{
		currtime=nPgmTime;
		newgyro = (float)HTGYROreadRot(HTGYRO);
		constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
		relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
		prevtime = currtime;
		wait1Msec(1);
		//-------------------------
		// IR
		//-------------------------
		bearingAC = HTIRS2readACDir(HTIRS2);

#define max(a, b)               (((a) > (b))? (a): (b))
#define min(a, b)               (((a) < (b))? (a): (b))

		currDir = (float) bearingAC;
		if (bearingAC == 0)
		{
			currDir = prevDir;
		}
		else
		{
			if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]))
			{
				bearingAC = (bearingAC - 1)/2;
				if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
				{
					currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
					max(acS[bearingAC], acS[bearingAC + 1]);
				}
			}
		}
		prevDir = currDir;
		////-------------------------
		//// Sonar
		////-------------------------
		//num = USreadDist(LEGOUS);
		//num2 = USreadDist(LEGOUS2);
		//if(num != 255) sonarLive = num;
		//if(num2 != 255) sonarLive2 = num2;
		//-------------------------
		// light
		//-------------------------
		LSsetActive(LEGOLS);
		nrm = LSvalNorm(LEGOLS);
	}
}
Beispiel #27
0
// main task
task main ()
{
	int _dirAC1 = 0;
	int _dirAC2 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;
	int acS12, acS22, acS32, acS42, acS52 = 0;
	int maxSig = 0;    // the max signal strength from the seeker.
	int maxSig2 = 0;

	// we are going to set DSP mode to 1200 Hz.
	tHTIRS2DSPMode _mode = DSP_1200;
	// attempt to set to DSP mode.
	if (HTIRS2setDSPMode(HTIRS2, _mode) == 0 || HTIRS2setDSPMode(ir2, _mode) == 0)
	{
		// unsuccessful at setting the mode.
		// display error message.
		eraseDisplay();
		nxtDisplayCenteredTextLine(0, "ERROR!");
		nxtDisplayCenteredTextLine(2, "Init failed!");
		nxtDisplayCenteredTextLine(3, "Connect sensor");
		nxtDisplayCenteredTextLine(4, "to Ports 1 and 4.");
		// make a noise to get their attention.
		PlaySound(soundBeepBeep);
		// wait so user can read message, then leave main task.
		wait10Msec(300);
		return;
	}

	eraseDisplay();
	// loop continuously and read from the sensor.
	while(true)
	{
		// read the current modulated signal direction
		_dirAC1 = HTIRS2readACDir(HTIRS2);
		_dirAC2 = HTIRS2readACDir(ir2);
		if (_dirAC1 < 0)
		{
			// error! - write to debug stream and then break.
			writeDebugStreamLine("Read dir ERROR!");
			break;
		}
		if (_dirAC2 < 0)
		{
			// error! - write to debug stream and then break.
			writeDebugStreamLine("Read dir ERROR!");
			break;
		}
		// Get the AC signal strength values.
		if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
		{
			// error! - write to debug stream and then break.
			writeDebugStreamLine("Read sig ERROR!");
			break;
			}  else {
			// find the max signal strength of all detectors.
		maxSig = (acS1 > acS2) ? acS1 : acS2;
		maxSig = (maxSig > acS3) ? maxSig : acS3;
		maxSig = (maxSig > acS4) ? maxSig : acS4;
		maxSig = (maxSig > acS5) ? maxSig : acS5;
		}
		if (!HTIRS2readAllACStrength(ir2, acS12, acS22, acS32, acS42, acS52 ))
		{
			// error! - write to debug stream and then break.
			writeDebugStreamLine("Read sig ERROR!");
			break;
			}  else {
			// find the max signal strength of all detectors.
		maxSig2 = (acS12 > acS22) ? acS12 : acS22;
		maxSig2 = (maxSig2 > acS32) ? maxSig2 : acS32;
		maxSig2 = (maxSig2 > acS42) ? maxSig2 : acS42;
		maxSig2 = (maxSig2 > acS52) ? maxSig2 : acS52;
		}

		// display info
		nxtDisplayCenteredTextLine(1, "DirL = %d", _dirAC1);
		nxtDisplayCenteredTextLine(4, "DirR = %d", _dirAC2);
		nxtDisplayCenteredTextLine(2, "SigL = %d", maxSig);
		nxtDisplayCenteredTextLine(5, "SigR = %d", maxSig2);
		// figure out which direction to go...
		// a value of zero means the signal is not found.
		// 1 corresponds to the far left (approx. 8 o'clock position).
		// 5 corresponds to straight ahead.
		// 9 corresponds to far right.
		// first translate directional index so 0 is straight ahead.
		// wait a little before resuming.
		wait10Msec(2);
	}
}
// main task
task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;

	// the default DSP mode is 1200 Hz.
  tHTIRS2DSPMode _mode = DSP_1200;

  // show the user what to do
  displayInstructions();

  while(true)
  {
    // You can switch between the two different DSP modes by pressing the
    // orange enter button

    PlaySound(soundBeepBeep);
    while(bSoundActive)
    {}
    eraseDisplay();
    nNumbCyles = 0;
    ++nInits;
    while (true)
    {
      if ((nNumbCyles & 0x04) == 0)
        nxtDisplayTextLine(0, "Initializing...");
      else
        nxtDisplayTextLine(0, "");
      nxtDisplayCenteredBigTextLine(1, "IR Seekr");

      // set the DSP to the new mode
      if (HTIRS2setDSPMode(HTIRS2, _mode))
        break; // Sensor initialized

      ++nNumbCyles;
      PlaySound(soundShortBlip);
      nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
      nxtDisplayCenteredTextLine(6, "Connect Sensor");
      nxtDisplayCenteredTextLine(7, "to Port S1");
      wait1Msec(100);
    }

    eraseDisplay();
    for (int i = 0; i < 8; ++i)
      sTextLines[i] = "";

    // display the current DSP mode
    if (_mode == DSP_1200)
      nxtDisplayTextLine(0, "      DC  1200");
    else
      nxtDisplayTextLine(0, "      DC   600");

    while (true)
    {
      ++nNumbCyles;
      if (nNxtButtonPressed == kEnterButton)
      {
        // "Enter" button has been pressed. Need to switch mode

        _mode = (_mode == DSP_1200) ?  DSP_600 : DSP_1200;
        while(nNxtButtonPressed == kEnterButton)
        {
          // Wait for "Enter" button release
        }
        break;
      }

      // Read the current non modulated signal direction
      _dirDC = HTIRS2readDCDir(HTIRS2);
      if (_dirDC < 0)
        break; // I2C read error occurred

      // read the current modulated signal direction
      _dirAC = HTIRS2readACDir(HTIRS2);
      if (_dirAC < 0)
        break; // I2C read error occurred

      // Read the individual signal strengths of the internal sensors
      // Do this for both unmodulated (DC) and modulated signals (AC)
      if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
        break; // I2C read error occurred
      if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
        break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);
      nxtDisplayTextLine(7, "Enter to switch");
    }
  }
}
// main task
task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
	int acS1, acS2, acS3, acS4, acS5 = 0;

  // show the user what to do
  displayInstructions();

  while(true)
  {
    PlaySound(soundBeepBeep);
    while(bSoundActive)
    {}
    eraseDisplay();
    nNumbCyles = 0;
    ++nInits;
    while (true)
    {
      if ((nNumbCyles & 0x04) == 0)
        nxtDisplayTextLine(0, "Initializing...");
      else
        nxtDisplayTextLine(0, "");
      nxtDisplayCenteredBigTextLine(1, "SMUX");

			// Before using the SMUX, you need to initialise the driver
			HTSMUXinit();

			// Tell the SMUX to scan its ports for connected sensors
			if (HTSMUXscanPorts(HTSMUX))
			  break;

      ++nNumbCyles;
      PlaySound(soundShortBlip);
      nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
      nxtDisplayCenteredTextLine(6, "Connect SMUX");
      nxtDisplayCenteredTextLine(7, "to Port S1");
      wait1Msec(100);
    }

    eraseDisplay();
    for (int i = 0; i < 8; ++i)
      sTextLines[i] = "";

    // display the current DSP mode
    // When connected to a SMUX, the IR Seeker V2 can only be
    // used in 1200Hz mode.
    nxtDisplayTextLine(0, "      DC  1200");

	  // The sensor is connected to the first port
	  // of the SMUX which is connected to the NXT port S1.
	  // To access that sensor, we must use msensor_S1_1.  If the sensor
	  // were connected to 3rd port of the SMUX connected to the NXT port S4,
	  // we would use msensor_S4_3

    while (true)
    {
      // Read the current non modulated signal direction
      _dirDC = HTIRS2readDCDir(msensor_S1_1);
      if (_dirDC < 0)
        break; // I2C read error occurred

      // read the current modulated signal direction
      _dirAC = HTIRS2readACDir(msensor_S1_1);
      if (_dirAC < 0)
        break; // I2C read error occurred

      // Read the individual signal strengths of the internal sensors
      // Do this for both unmodulated (DC) and modulated signals (AC)
      if (!HTIRS2readAllDCStrength(msensor_S1_1, dcS1, dcS2, dcS3, dcS4, dcS5))
        break; // I2C read error occurred
      if (!HTIRS2readAllACStrength(msensor_S1_1, acS1, acS2, acS3, acS4, acS5 ))
        break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);
      if (HTSMUXreadPowerStatus(HTSMUX))
        nxtDisplayTextLine(7, "Batt: bad");
      else
        nxtDisplayTextLine(7, "Batt: good");
    }
  }
}
Beispiel #30
0
task main()
{
	initializeRobot();

	int _dirDC_left = 0;
	int _dirAC_left = 0;

	int _dirDC_right = 0;
	int _dirAC_right = 0;

	int dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left = 0;
	int acS1_left, acS2_left, acS3_left, acS4_left, acS5_left = 0;

	int dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right = 0;
	int acS1_right, acS2_right, acS3_right, acS4_right, acS5_right = 0;

	eraseDisplay();
	for (int i = 0; i < 8; ++i)
		sTextLines[i] = "";

	waitForStart();

	tHTIRS2DSPMode _mode = DSP_1200;

	while(true) {
		PlaySound(soundShortBlip);
		if(HTIRS2setDSPMode(irL, _mode)) {
			break;
		}
	}
	while(true) {
		PlaySound(soundShortBlip);
		if(HTIRS2setDSPMode(irR, _mode)) {
			break;
		}
	}

	while (true) {
		nxtDisplayTextLine(1, "  L       R");

		_dirDC_left = HTIRS2readDCDir(irL);
		if (_dirDC_left < 0)
			break; // I2C read error occurred
		// read the current irL);
		_dirAC_left = HTIRS2readACDir(irR);
		if (_dirAC_left < 0)
			break; // I2C read error occurred

		_dirDC_right = HTIRS2readDCDir(irR);
		if (_dirDC_right < 0)
			break; // I2C read error occurred
		// read the current modulated signal direction
		_dirAC_right = HTIRS2readACDir(irR);
		if (_dirAC_right < 0)
			break; // I2C read error occurred

		if (!HTIRS2readAllDCStrength(irL, dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left))
			break; // I2C read error occurred
		if (!HTIRS2readAllACStrength(irL, acS1_left, acS2_left, acS3_left, acS4_left, acS5_left))
			break; // I2C read error occurred
		if (!HTIRS2readAllDCStrength(irR, dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right))
			break; // I2C read error occurred
		if (!HTIRS2readAllACStrength(irR, acS1_right, acS2_right, acS3_right, acS4_right, acS5_right))
			break; // I2C read error occurred

		displayText(1, "D", _dirAC_left, _dirAC_right);
		displayText(2, "0", acS1_left, acS1_right);
		displayText(3, "1", acS2_left, acS2_right);
		displayText(4, "2", acS3_left, acS3_right);
		displayText(5, "3", acS4_left, acS4_right);
		displayText(6, "4", acS5_left, acS5_right);
	}
}