task main () {
  nxtDisplayCenteredTextLine(0, "MindSensors");
  nxtDisplayCenteredBigTextLine(1, "TouchPnl");
  nxtDisplayCenteredTextLine(3, "Noise-a-Tron");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S3");
  wait1Msec(2000);
  eraseDisplay();

  int x, y = 0;
  ubyte buttons = 0;
  int multiplierX = 10;
  int multiplierY = 10;
  while (true) {
    if (!MSTPgetTouch(MSTP, x, y, buttons))
      PlaySound(soundBlip);
    else if (isButtonTouched(buttons, BUTTON_L1))
      multiplierX = 10;
    else if (isButtonTouched(buttons, BUTTON_L2))
      multiplierX = 15;
    else if (isButtonTouched(buttons, BUTTON_L3))
      multiplierX = 20;
    else if (isButtonTouched(buttons, BUTTON_L4))
      multiplierX = 25;
    else if (isButtonTouched(buttons, BUTTON_R1))
      multiplierY = 10;
    else if (isButtonTouched(buttons, BUTTON_R2))
      multiplierY = 15;
    else if (isButtonTouched(buttons, BUTTON_R3))
      multiplierY = 20;
    else if (isButtonTouched(buttons, BUTTON_R4))
      multiplierY = 25;

    else if (x > 0 && y > 0)
    {
      PlayImmediateTone(x * multiplierX, 1);
      wait1Msec(10);
      PlayImmediateTone(y * multiplierY, 1);
      nxtSetPixel(x, y);
    }
    EndTimeSlice();
  }
}

task main()
{
    int basketNumber;
    bool done = false;

    initializeRobot();

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

    wait1Msec(10000);

    disableDiagnosticsDisplay();
    eraseDisplay();

    basketNumber = findIRBeacon();
    nxtDisplayTextLine(5, "Beacon #%d", basketNumber);

    wait1Msec(300);

    if (basketNumber != -1) {
        if (compensation[basketNumber] < 0) {
            moveForward(abs(compensation[basketNumber]), 60);
        } else {
            moveBackward(compensation[basketNumber], 60);
        }
        /*for (int i = 0; i <= basketNumber; i++) {
            displayCaution();
            wait1Msec(500);
            displayBackward();
            wait1Msec(500);
        }
        displayRestingPulse();
        */
        dumpBlock();
    } else {
        basketNumber = 3;
    }

    //moveToRamp(basketNumber);

    while (true)
    {}
}

/*===================================================================================================================*/
task main() {

   int myServo;

   eraseDisplay();
   myServo = 250;
   servoTarget[servo1] = myServo;

   while(true) {
      if (nNxtButtonPressed == 1) {
         myServo = myServo + 5;
      } else if (nNxtButtonPressed == 2) {
         myServo = myServo - 5;
      }
      servoTarget[servo1] = myServo;
      nxtDisplayCenteredBigTextLine(3, "%d", ServoValue[servo1]);
      wait10Msec(2);
   }
}

// Stop the calibration and complain loudly if somethign goes wrong
void stopCalibration() {
  if (!HTMCstopCal(HTCOMPASS)) {
    eraseDisplay();
    nxtDisplayTextLine(1, "ERROR: Calibration");
    nxtDisplayTextLine(2, "has failed.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive) EndTimeSlice();
    wait1Msec(5000);
    StopAllTasks();
  } else {
    nxtDisplayTextLine(1, "SUCCESS: ");
    nxtDisplayTextLine(2, "Calibr. done.");
    PlaySound(soundUpwardTones);
    while(bSoundActive) EndTimeSlice();
    wait1Msec(5000);
  }
}

task main()
{
  long rate = 0;
  eraseDisplay();
  bNxtLCDStatusDisplay = true; // Enable top status line display
  writeDebugStream("Scanning for wifi sensor: ");
  rate = scanBaudRate();
  writeDebugStreamLine("%d baud", rate);
  configureWiFi();
  set_verbose(false);
  Receive();
  wait1Msec(100);
  closeAllConns();
  wait1Msec(1000);
  clear_read_buffer();
  startListen(80);
  clear_read_buffer();
  parseInput();
}

task main() {
  byte inputs = 0;

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

  while(true) {
    eraseDisplay();
    // Fetch the state of the digital IO pins.  When not explicitly
    // configured as input or output, they will default to input.
    inputs = HTPBreadIO(HTPB, 0x3F);
    nxtDisplayTextLine(4, "D: 0x%x", ubyteToInt(inputs));
    wait1Msec(100);
  }
}

////////////////////////////////////////////////////////////////////////////////
//Select Robot Start Location (Wall) -----------------------------------------//
////////////////////////////////////////////////////////////////////////////////
void selectStartLocation()
{
	eraseDisplay();
	// Enable Top Line NXT Display (Bluetooth, NXT Name & Battery Status)
	//bNxtLCDStatusDisplay = true;
	while(true)
	{
		nxtDisplayCenteredTextLine(1,"DITU SAYS WEAR");
		nxtDisplayCenteredTextLine(2,"SAFETY GLASSES");
		nxtDisplayCenteredTextLine(4,"Start Location:");
		//------------------------------------------------
		if(nNxtButtonPressed == 1)	// NXT Right Triangle Button
		{
			robotStartLocation++;
			wait1Msec(400);
		}
		//-------------------------------------------------
		if(nNxtButtonPressed == 2)	// NXT Left Triangle Button
		{
			robotStartLocation--;
			wait1Msec(400);
		}
		// Keep AutoActions within a range of valid values
		if (robotStartLocation > 2) robotStartLocation = 2;
		if (robotStartLocation < 0) robotStartLocation = 0;
		//-------------------------------------------------
		switch (robotStartLocation)
		{
		case 0: nxtDisplayCenteredTextLine(6, "Left Wall");break;
		case 1: nxtDisplayCenteredTextLine(6, "Corner");break;
		case 2: nxtDisplayCenteredTextLine(6, "Right Wall");break;
		case 3: nxtDisplayCenteredTextLine(6, "Under Rack");break;
		default: nxtDisplayCenteredTextLine(6, "Error");break;
		}
		//---------------------------------------------------------------
		if(nNxtButtonPressed == 3)	// NXT Orange Button
		{
			wait1Msec(400);
			break;
		}
		//---------------------------------------------------------------
	}
}

task main() {
  int raw = 0;
  int nrm = 0;
  bool active = true;
  LSsetActive(LEGOLS);

  nNxtButtonTask  = -2;

  eraseDisplay();
  nxtDisplayTextLine(0, "Light Sensor");
  nxtDisplayTextLine(2, "Press orange");
  nxtDisplayTextLine(3, "button to switch");

  while (true) {
    // The enter button has been pressed, switch
    // to the other mode
    if (nNxtButtonPressed == kEnterButton) {
      active = !active;
      if (!active)
        // Turn the light off
        LSsetInactive(LEGOLS);
      else
        // Turn the light on
        LSsetActive(LEGOLS);

      // wait 500ms to debounce the switch
      wait1Msec(500);
    }

    nxtDisplayClearTextLine(5);
    nxtDisplayClearTextLine(6);

    // Get the raw value from the sensor
    raw = LSvalRaw(LEGOLS);

    // Get the normalised value from the sensor
    nrm = LSvalNorm(LEGOLS);

    nxtDisplayTextLine(5, "Raw:  %4d", raw);
    nxtDisplayTextLine(6, "Norm: %4d", nrm);
    wait1Msec(50);
  }
}

task main () {
  int _dir = 0;
	int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "IR Seekr");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "This is for the");
  nxtDisplayCenteredTextLine(6, "v1 seeker");
  wait1Msec(2000);

  while(true) {
    eraseDisplay();

    // read all of the sensors' values at once,
    // exit the app if an error occurs
    if (! HTIRSreadAllStrength(HTIRS, dcS1, dcS2, dcS3, dcS4, dcS5)) {
      nxtDisplayTextLine(4, "ERROR!!");
      wait1Msec(2000);
      StopAllTasks();
    }

    // read the direction from which the signal is coming,
    // exit the app if an error occurs
    _dir = HTIRSreadDir(HTIRS);
    if (_dir < 0) {
      nxtDisplayTextLine(4, "ERROR!!");
      wait1Msec(2000);
      StopAllTasks();
    }

    // display the info from the sensor
    nxtDisplayTextLine(0,"HT IR Seeker");
    nxtDisplayTextLine(2, "dir: %2d", _dir);
    nxtDisplayTextLine(3, "S1: %3d", dcS1);
    nxtDisplayTextLine(4, "S2: %3d", dcS2);
    nxtDisplayTextLine(5, "S3: %3d", dcS3);
    nxtDisplayTextLine(6, "S4: %3d", dcS4);
    nxtDisplayTextLine(7, "S5: %3d", dcS5);
    wait10Msec(100);
  }
}

////////////////////////////////////////////////////////////////////////////////
//Select Row------------------------------------------------------------------//
////////////////////////////////////////////////////////////////////////////////
void selectRow()
{
	eraseDisplay();
	// Enable Top Line NXT Display (Bluetooth, NXT Name & Battery Status)
	//bNxtLCDStatusDisplay = true;
	while(true)
	{
		nxtDisplayCenteredTextLine(1,"DITU SAYS WEAR");
		nxtDisplayCenteredTextLine(2,"SAFETY GLASSES");
		nxtDisplayCenteredTextLine(4,"Peg Row:");
		//------------------------------------------------
		if(nNxtButtonPressed == 1)	// NXT Right Triangle Button
		{
			Row++;
			wait1Msec(400);
		}
		//-------------------------------------------------
		if(nNxtButtonPressed == 2)	// NXT Left Triangle Button
		{
			Row--;
			wait1Msec(400);
		}
		//---------------------------------------------------------------
		// Keep Row within a range of valid values
		if (Row > 2) Row = 2;
		if (Row < 0) Row = 0;
		switch (Row)
		{
		case 0: nxtDisplayCenteredTextLine(6, "Bottom Row");break;
		case 1: nxtDisplayCenteredTextLine(6, "Middle Row");break;
		case 2: nxtDisplayCenteredTextLine(6, "Top Row");break;
		default: nxtDisplayCenteredTextLine(6, "Error");break;
		}
		//---------------------------------------------------------------
		if(nNxtButtonPressed == 3)	// NXT Orange Button
		{																																				////////////////	You  need to return a variable
			wait1Msec(400);																										////////////////	that selects the auto program
			break;																																////////////////	for later. 			-Jos :)
		}
		//---------------------------------------------------------------
	}
}

task main() {
  nxtDisplayCenteredTextLine(0, "Lego");
  nxtDisplayCenteredBigTextLine(1, "TOUCH");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);

  eraseDisplay();

  while (true) {
    // Check if the sensor is pressed or not.
    if (TSreadState(LEGOTOUCH))
      nxtDisplayCenteredBigTextLine(3, "ACTIVE");
    else
      nxtDisplayCenteredBigTextLine(3, "INACTIVE");
    wait1Msec(50);
  }
}

void position(){
	nxtDisplayTextLine(1, "L_ARROW = ARM FRIST");
	nxtDisplayTextLine(2, "R_ARROW = HAND FIRST");
	nxtDisplayTextLine(5, "WHICH SIDE?");
	bool select = false;
	while(!select){
		if(nNxtButtonPressed == 2){         //Left orange is button 2
			wait10Msec(50);
			r = -1;
			select = true;
			}else if(nNxtButtonPressed == 1){    // Right orange is button 1
			wait10Msec(50);
			r = +1;
			select = true;
		}
	}
	wait10Msec(100);
	eraseDisplay();
	nxtDisplayCenteredBigTextLine(4, "READY!");
}

/**
 * This displays an arrow on the screen pointing downwards.
 * @param degreesFromDown the number of degrees from down
 */
void displayArrow(int degreesFromDown)
{
   eraseDisplay();
   // Otherwise, the arrow would point up.
   degreesFromDown = degreesFromDown-180;

   //If you don't know trigonometry, you can ignore this part
   nxtDrawLine(49,
               31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
   nxtDrawLine((cosDegrees(degreesFromDown - 20) * 15) + 49,
               (sinDegrees(degreesFromDown - 20) * 15) + 31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
   nxtDrawLine((cosDegrees(degreesFromDown + 20) * 15) + 49,
               (sinDegrees(degreesFromDown + 20) * 15) + 31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
}

task main () {
  string w_string = "MindstormsNXT";
  string r_string;
  tByteArray data;

  memcpy(data, w_string, sizeof(w_string));
  EEPROMwriteBytes(EEPROM, 0, data, 13);
  // After a write, you *must* wait 5 msec
  sleep(5);

  eraseDisplay();
  memset(data, 0, sizeof(tByteArray));
  EEPROMreadBytes(EEPROM, 0, data, 13);
  // Make sure the string is 0 terminated.
  data[13] = 0;
  StringFromChars(r_string, &data[0]);

  displayTextLine(1, "%s", r_string);
  sleep(2000);
}

task main()
{
    done = false;

	eraseDisplay();

    nMotorEncoder[driveRight] = 0;
    nMotorEncoder[driveLeft] = 0;

	HTMCstartCal(HTMC);
	StartTask(rotate);

    while (!done) {
        nxtDisplayTextLine(2, "Right: %d", nMotorEncoder[driveRight]);
        nxtDisplayTextLine(3, "Left:  %d", nMotorEncoder[driveLeft]);
    }
	HTMCstopCal(HTMC);

    while (true) {}
}

task main() {
  calibrateLight();
  calibrateCompass();

  int found = FollowSegmentTilEnd();
  motor[left] = 0;
  motor[right] = 0;
  eraseDisplay();
  if(found == FOUND_NODE) {
    nxtDisplayCenteredTextLine(3, "Segment end");
  }
  else if(found == FOUND_CAN) {
    nxtDisplayCenteredTextLine(3, "Can Detected");
  }
  else if(FOUND_ERROR) {
    nxtDisplayCenteredTextLine(3, "Error");
  }

  wait10Msec(1000);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void initializeRobot()
{
	//Initialize the position of the servos that control our ring arm
	servo[pivotLeft] = 0;
	servo[pivotRight] = 255;

	//By default the NXT display will show diagnostics. This line disables that so we can display what
	//we want to
	disableDiagnosticsDisplay();
	eraseDisplay();
	nxtDisplayCenteredTextLine(1, "Autonomous");
	nxtDisplayTextLine(2, gRun);  //Sets line 2 of the display to show what routine is currently selected

	nNxtButtonTask = ButtonTask;  //Starts up the button task defined above for selecting the auto routine
	nNxtExitClicks = 2; //Normally the gray NXT button exits the program, but since we use the exit button
	//to "back up", we reset the exit clicks to 2 so that a double click will exit the
	//program

	while (gReady < 5)
	{
		//Do nothing. This loop puts us in a wait mode while a team member selects the program,
		//alliance color, and position of the robot, etc using the button task above
	}

	nxtDisplayCenteredTextLine(7, "READY"); //Selection is done so display ready on the screen

	//Calibrate the gyro sensor while the robot is still
	HTGYROstartCal(gyro);
	wait1Msec(1000);

	//In order to speed up the sensing of the IR beacon, we initialize the servo for the IR seeker based
	//on which side the robot was placed on
	if (strcmp(gSide, "left")== 0)
	{
		servo[IR] = 50;
	}
	else
	{
		servo[IR] = 175;
	}
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.
    nMotorEncoder[tower] = 0;

    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
           case 0:
                // step 0: go forward.
                SetDriveTarget(60.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
								if (g_driveEnabled == false)
                {
                step++;
                }
                break;

        }

        ArmTask();
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

void configureWiFi(char *_ssid, char *_key)
{
  short len;
  RS485clearRead();      // Clear out the buffer and test TX/RX.
  sleep(1000);          // Must be run first!
  DWIFIsetEcho(false);     // Must be run first!
  sleep(1000);          // Must be run first!
  RS485clearRead();
  eraseDisplay();
  DWIFIsetSWControl();  // Must be run first!
  RS485clearRead();
  DWIFIsetVerbose(false);
  sleep(100);
  RS485clearRead();
  RS485read(RS485rxbuffer, len);
  DWIFIsetSSID(_ssid);
  DWIFIsetDHCP(true);
  DWIFIsetWPAPSK(_ssid, _key);
  sleep(20000);
  DWIFIsaveConfig();
}

task main(){

  int x_val, y_val, z_val;      // axis values
  float x_fval, y_fval, z_fval;      // axis values

  nxtDisplayCenteredTextLine(0, "Mindsensors");
  nxtDisplayCenteredBigTextLine(1, "IMU");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);
  eraseDisplay();

  /*
 	 * Range 1: Change Accelerometer Sensitivity to 2G and Gyro to 250 degrees/sec
 	 * Range 2: Change Accelerometer Sensitivity to 4G and Gyro to 500 degrees/sec
 	 * Range 3: Change Accelerometer Sensitivity to 8G and Gyro to 2000 degrees/sec
   * Range 4: Change Accelerometer Sensitivity to 16G and Gyro to 2000 degrees/sec
 	 */

  MSIMUsetRange(MSIMU, MSIMU_SENSITIVITY_RANGE_1);

  while (true){

		// Read the GYROSCOPE
    MSIMUreadGyroAxes(MSIMU, x_fval, y_fval, z_fval);

		nxtDisplayTextLine(1, "%.2f", x_fval);
		nxtDisplayTextLine(2, "%.2f", y_fval);
		nxtDisplayTextLine(3, "%.2f", z_fval);

		// Read the accelerometer
		MSIMUreadAccelAxes(MSIMU, x_val, y_val, z_val);

		nxtDisplayTextLine(5, "%d", x_val);
		nxtDisplayTextLine(6, "%d", y_val);
		nxtDisplayTextLine(7, "%d", z_val);
		wait1Msec(50);
  }
}