#pragma config(Sensor, S1,     HTCS2,          sensorI2CCustom)
#pragma config(Motor,  motorA,          LEFT,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          RIGHT,         tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
* Xander Soldaat (xander_at_botbench.com)
* 11 April 2013
* version 0.1
*/

#include "drivers/hitechnic-colour-v2.h"

task main () {
  int _color = 0;

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "Color V2");
  nxtDisplayCenteredTextLine(3, "Test 3");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);

  eraseDisplay();

  // Start the motors
  motor[LEFT]  = 50;
  motor[RIGHT] = 50;

  while (true) {

    // Read the currently detected colour from the sensor
    // You can find a list of all the colours here:
    // http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NCO1038
    _color = HTCS2readColor(HTCS2);

    // If colour == -1, it implies an error has occurred
    if (_color < 0) {
      nxtDisplayTextLine(4, "ERROR!!");
      wait1Msec(2000);
      StopAllTasks();
    }

    nxtDisplayCenteredTextLine(0, "Color: %d", _color);

    // if the colour is red (8), stop the motors and exit while loop
    if (_color == 8)
    {
      motor[LEFT]  = 0;
      motor[RIGHT] = 0;
      PlaySound(soundBeepBeep);
      while(bSoundActive)
      {
        wait1Msec(100);
      }
      break;
    }
    wait1Msec(50);
  }
}