#pragma config(Sensor, S1,     TOUCH,          sensorTouch)
#pragma config(Motor,  motorA,          M_Y,           tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          M_X,           tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          M_CONTROL,     tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
 * Glidewheel-demo1.c is a small program to demonstrate the Mindsensors GlideWheel-M
 * Power Functions motor encoders.
 *
 * Changelog:
 * - 0.1: Initial release
 *
 * Credits:
 * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where it's due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 2.00 AND HIGHER.
 * Xander Soldaat (mightor_at_gmail.com)
 * 28 June 2012
 * version 0.1
 */

task main ()
{
  // Local variables
  bool leftright = true;
  long previous_encoder = 0;
  float delta_encoder = 0;
  float rpm;

  previous_encoder = nMotorEncoder[M_CONTROL];
  time1[T1] = 0;
  nxtDisplayTextLine(7, "  X    POS    Y");
  while (true)
  {
    // Reset to the NULL position.
    if (nNxtButtonPressed == kEnterButton)
    {
      PlaySound(soundFastUpwardTones);
      // Debounce
      while (nNxtButtonPressed != kNoButton) EndTimeSlice();
      motor[M_X] = sgn(nMotorEncoder[M_X]) * -6;
      while (abs(nMotorEncoder[M_X]) != 0) EndTimeSlice();
      motor[M_X] = 0;
      motor[M_Y] = sgn(nMotorEncoder[M_Y]) * -6;
      while (abs(nMotorEncoder[M_Y]) != 0) EndTimeSlice();
      motor[M_Y] = 0;
      PlaySound(soundDownwardTones);
    }
    // Reset the Y encoder
    else if (nNxtButtonPressed == kRightButton)
    {
      PlaySound(soundBlip);
      // debounce
      while (nNxtButtonPressed != kNoButton) EndTimeSlice();
      nMotorEncoder[M_Y] = 0;
    }
    // Reset the X encoder
    else if (nNxtButtonPressed == kLeftButton)
    {
      PlaySound(soundBlip);
      //debounce
      while (nNxtButtonPressed != kNoButton) EndTimeSlice();
      nMotorEncoder[M_X] = 0;
    }

    // Switch between motors
    if (SensorValue[TOUCH] == 1)
    {
      while (SensorValue[TOUCH] == 1) EndTimeSlice();
      leftright = !leftright;
      PlaySound(soundBeepBeep);
    }
    wait1Msec(50);
    delta_encoder = nMotorEncoder[M_CONTROL] - previous_encoder;
    previous_encoder = nMotorEncoder[M_CONTROL];
    rpm = (delta_encoder * (1000.0 / time1[T1])) / 60;
    time1[T1] = 0;
    // determine the motor speed, based on the RPM of motor C
    if (leftright)
    {
      motor[M_X] = rpm * 2;
      motor[M_Y] = 0;
      nxtDisplayCenteredBigTextLine(1, "X AXIS");
    }
    else
    {
      motor[M_Y] = rpm * 2;
      motor[M_X] = 0;
      nxtDisplayCenteredBigTextLine(1, "Y AXIS");
    }
    // Some info on the screen, always useful.
    nxtDisplayCenteredBigTextLine(3, "%3.2f", rpm);
    nxtDisplayTextLine(5, "X: %d", nMotorEncoder[M_X]);
    nxtDisplayTextLine(6, "Y: %d", nMotorEncoder[M_Y]);
  }
}