const tSensors LightSensor          = (tSensors) S3;   //sensorLightActive  //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//NXTSegway_Rev_01.c -- Using the NXT's raw light sensor values to more precisely calculate error
//and compensate for it using the motors to achieve and sustain stability

int midValue = 0;
int minValue = 0;
int maxValue = 0;
task main()
{

   nSyncedMotors = synchAC;

   bFloatDuringInactiveMotorPWM = false;

   while (nNxtButtonPressed == -1)
   {
   }

   midValue = SensorRaw[LightSensor];

   wait1Msec(1000);

   while (nNxtButtonPressed == -1)
   {
   }

   minValue = SensorRaw[LightSensor];

   wait1Msec(1000);

   while (nNxtButtonPressed == -1)
   {
   }

   maxValue = SensorRaw[LightSensor];

   wait1Msec(1000);

   if (maxValue < midValue && minValue < midValue)
   {
      PlaySound(soundDownwardTones);
      if (bSoundActive == true)
      {
      }
      nxtDisplayCenteredTextLine(4, "Choose somewhere else");
   }

   if (maxValue > midValue && minValue > midValue)
   {
      PlaySound(soundDownwardTones);
      if (bSoundActive == true)
      {
      }
      nxtDisplayCenteredTextLine(4, "Choose somewhere else");
   }

   if (maxValue > minValue)
   {
      bMotorReflected[motorA] = true;
      bMotorReflected[motorC] = true;
   }

   while (nNxtButtonPressed == -1)
   {
   }

   wait1Msec(3000);

   while (SensorRaw[LightSensor] == midValue)
   {
   }

   while (true)
   {
      if (SensorRaw[LightSensor] < midValue)
      {
         while (SensorRaw[LightSensor] < midValue)
         {
            motor[motorA] = 100;
         }
      }
      else
      {
         while (SensorRaw[LightSensor] > midValue)
         {
            motor[motorA] = -100;
         }
      }
   }
}

//Results -- Very jerky, but stays up for 4-5 seconds at a time and the error does not
//continuously escalate to extremes that are unable to be compensated for like revision 0;
//much quicker response time

//*!!Sensor,    S3,          LightSensor, sensorLightActive,      ,              !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors LightSensor          = (tSensors) S3;   //sensorLightActive  //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//NXTSegway_Rev_02.c -- Using the NXT's raw light sensor values to more precisely calculate error
//and compensate for it using the motors and dynamic recalibration to stabilize itself

int midValue = 0;      //Initial vertical raw value
int minValue = 0;      //Initial tilted-back raw value
int maxValue = 0;      //Initial tilted-forward raw value
int extrmValue1 = 0;      //Stores extremes
int extrmValue2 = 0;      //Stores extremes
int avgValue = 0;      //Dynamically-adjusted vertical value
int n = 0;      //Counter

task main()
{

   nSyncedMotors = synchAC;      //synchs both motors

   bFloatDuringInactiveMotorPWM = false;      //Motors brake when inactive

   while (nNxtButtonPressed == -1)      //Waits for a button press
   {
   }

   midValue = SensorRaw[LightSensor];      //Reads and stores the vertical raw light data

   wait1Msec(1000);      //nNXTButtonPressed can reset to -1

   while (nNxtButtonPressed == -1)      //Waits for a button press
   {
   }

   minValue = SensorRaw[LightSensor];      //Reads and stores the tilted raw light data

   wait1Msec(1000);      //nNXTButtonPressed can reset to -1

   while (nNxtButtonPressed == -1)      //Waits for button press
   {
   }

   maxValue = SensorRaw[LightSensor];      //Reads and stores the tilted raw light data

   wait1Msec(1000);      //nNXTButtonPressed can reset to -1

   if (maxValue < midValue && minValue < midValue)      //Erratic light patterns handler
   {
      PlaySound(soundDownwardTones);
      if (bSoundActive == true)
      {
      }
      nxtDisplayCenteredTextLine(4, "Choose somewhere else");
   }

   if (maxValue > midValue && minValue > midValue)      //Erratic light patterns handler
   {
      PlaySound(soundDownwardTones);
      if (bSoundActive == true)
      {
      }
      nxtDisplayCenteredTextLine(4, "Choose somewhere else");
   }

   if (maxValue > minValue)      //Reverses motors depending on tilt direction
   {
      bMotorReflected[motorA] = true;
      bMotorReflected[motorC] = true;
   }

   while (nNxtButtonPressed == -1)      //Waits for a button press
   {
   }

   wait1Msec(3000);      //Waits three seconds before beginning

   avgValue = midValue;      //Sets the avgValue to the vertical value

   while (SensorRaw[LightSensor] == avgValue)      //If the robot is vertical, do nothing
   {
   }

   while (true)      //Forever
   {
      if (SensorRaw[LightSensor] < avgValue)      //If tilting
      {
         n = n + 1;      //Increment n

         extrmValue1 = SensorRaw[LightSensor];      //Store next extreme

         if (n == 1)      //If first loop...
         {
            extrmValue2 = avgValue - (extrmValue1 - avgValue);      //...sets second extreme to offset the first one
         }

         while (SensorRaw[LightSensor] < avgValue)      //While not vertical...
         {
            motor[motorA] = 100;      //...Power both motors until vertical
         }

         avgValue = (extrmValue2 + extrmValue1)/2;      //Find average of last two extremes

         extrmValue2 = SensorRaw[LightSensor];      //Store next extreme

         while (SensorRaw[LightSensor] > avgValue)      //While not vertical...
         {
            motor[motorA] = -100;      //...Power both motors until vertical
         }

         avgValue = (extrmValue1 + extrmValue2)/2;      //Finds average of last two extremes
      }
      else      //If tilting the other way
      {
         n = n + 1;      //Increment n

         extrmValue1 = SensorRaw[LightSensor];      //Store next extreme

         if (n == 1)      //If first loop...
         {
            extrmValue2 = avgValue - (extrmValue1 - avgValue);      //...sets second extreme to offset the first one
         }

         while (SensorRaw[LightSensor] > avgValue)      //While not vertical...
         {
            motor[motorA] = -100;      //...power both motors until vertical
         }

         avgValue = (extrmValue2 + extrmValue1)/2;      //finds average of last two extremes (new avgValue)

         extrmValue2 = SensorRaw[LightSensor];      //Store next extreme

         while (SensorRaw[LightSensor] < avgValue)      //While not vertical...
         {
            motor[motorA] = 100;      //...Power both motors until vertical
         }

         avgValue = (extrmValue1 + extrmValue2)/2;      //Finds average of last two extremes
      }
   }
}

//Results: Stays vertical independently for about 11 seconds on the right surface
//Problems: With current implementation of dynamic recalibration, significant error cannot be overcome