//=====================================================================
// HiTechnic HTWay Version 1.1
// Copyright (c) 2010 HiTechnic
//
// Revision History
//   1.1    Fixed a bug that prevented the HTWayC program from working
//          correctly when optimization level was set to 2.
//          Also imporived the accuracy of the initial gyro offset
//          caluculation by ensuring the motor controller in active.
//
// Gyro based sample Segway type robot. This NXC program uses the LEGO
// 2.0 firmware which has floating point support.  It has been tested
// with LEGO NXT firmware 1.29.
//
// To use the floating point math available in the 2.0 firmware,
// BricxCC must be setup to tell the compiler to target that firmware.
// From the Edit menu, select Preferences.  Select the Compiler tab,
// and then NBC/NXC sub-tab and check 'NXT 2.0 compatible firmware.'

// Port Input and Output
#define GYRO         IN_1
//#define IR_RECEIVER  IN_4

#define LEFT_MOTOR   OUT_C
#define RIGHT_MOTOR  OUT_A
#define MOTORS       OUT_AC

//=====================================================================
// Balancing constants
//
// These are the constants used to maintain balance.
//
// Loop wait time.  WAIT_TIME is the time in ms passed to the
// Wait command.
#define WAIT_TIME    8

// Wheel ratio is relative size compared to NXT 1.0 wheels
// For the NXT 1.0 wheels (56x26) use:           1.0
// For the NXT 2.0 wheels (43.2x22) use:         0.7
// For the big white RCX wheels (81.6x15) use:   1.4
//#define WHEEL_RATIO_NXT1 1.0
#define WHEEL_RATIO_NXT2 0.8
//#define WHEEL_RATIO_RCX  1.4

// These are the main four balance constants, only the gyro
// constants are relative to the wheel size.  KPOS and KSPEED
// are self-relative to the wheel size.
#define KGYROANGLE  7.5
#define KGYROSPEED  1.15
#define KPOS        0.07
#define KSPEED      0.1

// This constant aids in drive control.  When the robot starts
// moving because of user control, this constant helps get the
// robot leaning in the right direction.  Similarly, it helps
// bring robot to a stop when stopping.
#define KDRIVE      -0.02

// Power differential used for steering based on difference of
// target steering and actual motor difference.
#define KSTEER      0.25

// Gyro offset control
// The gyro sensor will drift with time.  This constant is used in a
// simple long term averaging to adjust for this drift.
// Every time through the loop, the current gyro sensor value is
// averaged into the gyro offset weighted according to this constant.
#define EMAOFFSET 0.0005

// If robot power is saturated (over +/- 100) for over this time limit
// then robot must have fallen.  In milliseconds.
#define TIME_FALL_LIMIT     1000

//---------------------------------------------------------------------
// IR Control constants
#define CONTROL_WAIT 25

// To use a different IR channel, chage these constants
//#define IR_LEFT           HT_CH1_A
//#define IR_RIGHT          HT_CH1_B

// This constant is in degrees/second for maximum speed.  Note that
// position and speed are measured as the sum of the two motors, in
// other words, 600 would actually be 300 degrees/second for each
// motor.
#define CONTROL_SPEED  600.0

//=====================================================================
// Global variables
//
// These variables are used to control the movement of the robot.  Both
// are in degrees/second.
//
// motorControlDriveis the target speed for the sum of the two motors
// while motorControlSteer is the target change in difference for two motors.
float motorControlDrive = 0.0;
float motorControlSteer = 0.0;

// This global contains the target motor differential, essentially,
// which way the robot should be pointing.  This value is
// updated every time through the balance loop based on motorControlSteer
float motorDiffTarget = 0.0;

// Time that robot first starts to balance.  Used to calculate tInterval
long tCalcStart;

// tInterval is the time, in seconds, for each iteration of the
// balance loop.
float tInterval;

// ratioWheel stores the relative wheel size compared to a standard NXT
// 1.0 wheel.  RCX 2.0 wheel has ratio of 0.7 while large RCX wheel is 1.4
float ratioWheel=WHEEL_RATIO_NXT2;

// Gyro globals
float gOffset;
float gAngleGlobal = 0;

// Motor globals
float motorPos = 0;
long mrcSum = 0, mrcSumPrev;
long motorDiff;
long mrcDeltaP3 = 0;
long mrcDeltaP2 = 0;
long mrcDeltaP1 = 0;



//=====================================================================
// GetGyroOffset
//
// This function returns a suitable initial gyro offset.  It takes
// 100 gyro samples over a time of 1/2 second and averages them to
// get the offset.  It also check the max and min during that time
// and if the difference is larger than one it rejects the data and
// gets another set of samples.
#define OFFSET_SAMPLES 100

void GetGyroOffset()
{
  float gSum;
  int  i, gMin, gMax, g;

  ClearScreen();
  TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");

  TextOut(0, LCD_LINE4, "Lay robot down");
  TextOut(0, LCD_LINE5, "flat to get gyro");
  TextOut(0, LCD_LINE6, "offset.");
  
  // Ensure that the motor controller is active since this affects
  // the gyro values.
  Off(MOTORS);
  
  do {
    gSum = 0.0;
    gMin = 1000;
    gMax = -1000;
    for (i=0; i<OFFSET_SAMPLES; i++) {
      g = SensorHTGyro(GYRO);

      if (g > gMax)
        gMax = g;
      if (g < gMin)
        gMin = g;

      gSum += g;
      Wait(5);
    }
  } while ((gMax - gMin) > 1);   // Reject and sample again if range too large

  //Average the sum of the samples.
  gOffset = gSum / OFFSET_SAMPLES + 1.0;
  
  // Even with motor controller active, the initial offset appears to
  // be off from the actual needed offset to keep robot from wondering.
  // This +1 helps keep robot from wondering when it first starts to
  // balance.
}

//=====================================================================
void StartBeeps()
{
  int c;

  ClearScreen();
  TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");

  TextOut(20, LCD_LINE3, "Balance in");

  // Play warning beep sequence to indicate balance about to start
  for (c=5; c>0;c--) {
    NumOut(47, LCD_LINE4, c);
    PlayTone(440,100);
    Wait(1000);
  }
  NumOut(47, LCD_LINE4, 0);
  PlayTone(440,1000);
  Wait(1000);
}

//=====================================================================
// task main - initializes the sensors, Both taskBalance and
// taskController start when main ends.
task main()
{
  // Initialize the sensors
  SetSensorHTGyro(GYRO);
//  SetSensorLowspeed(IR_RECEIVER);
  Wait(50);


  // Get the initial gyro offset
  GetGyroOffset();

  // Play warning beep sequence before balance starts
  StartBeeps();

  // When task main ends, both taskBalance and taskControl will start
}

//=====================================================================
// GetGyroData(float &gyroSpeed, float &gyroAngle)
//
// Get the data from the gyro.
// Fills the pass by reference gyroSpeed and gyroAngle based on
// updated information from the Gyro Sensor
//
// Maintains an automatically adjusted gyro offset as well as
// the integrated gyro angle.
inline void GetGyroData(float &gyroSpeed, float &gyroAngle)
{
  float gyroRaw;

  gyroRaw = SensorHTGyro(GYRO);
  gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset;
  gyroSpeed = gyroRaw - gOffset;

  gAngleGlobal += gyroSpeed*tInterval;
  gyroAngle = gAngleGlobal;
  

}

//---------------------------------------------------------------------

inline void GetMotorData(float &motorSpeed, float &motorPos)
{
  long mrcLeft, mrcRight, mrcDelta;

  // Keep track of motor position and speed
  mrcLeft = MotorRotationCount(LEFT_MOTOR);
  mrcRight = MotorRotationCount(RIGHT_MOTOR);

  // Maintain previus mrcSum so that delta can be calculated and get
  // new mrcSum and Diff values
  mrcSumPrev = mrcSum;
  mrcSum = mrcLeft + mrcRight;
  motorDiff = mrcLeft - mrcRight;

  // mrcDetla is the change int sum of the motor encoders, update
  // motorPos based on this detla
  mrcDelta = mrcSum - mrcSumPrev;
  motorPos += mrcDelta;

  // motorSpeed is based on the average of the last four delta's.
  motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval);

  // Shift the latest mrcDelta into the previous three saved delta values
  mrcDeltaP3 = mrcDeltaP2;
  mrcDeltaP2 = mrcDeltaP1;
  mrcDeltaP1 = mrcDelta;
}

//---------------------------------------------------------------------
// void SteerControl(int power, int &powerLeft, int &powerRight)
//
// This function determines the left and right motor power that should
// be used based on the balance power and the steering control
inline void SteerControl(int power, int &powerLeft, int &powerRight)
{
  int powerSteer;

  // Update the target motor difference based on the user steering
  // control value.
  motorDiffTarget += motorControlSteer * tInterval;

  // Determine the proportionate power differential to be used based
  // on the difference between the target motor difference and the
  // actual motor difference.
  powerSteer = KSTEER * (motorDiffTarget - motorDiff);

  // Apply the power steering value with the main power value to
  // get the left and right power values.
  powerLeft = power + powerSteer;
  powerRight = power - powerSteer;

  // Limit the power to motor power range -100 to 100
  if (powerLeft > 100)   powerLeft = 100;
  if (powerLeft < -100)  powerLeft = -100;

  // Limit the power to motor power range -100 to 100
  if (powerRight > 100)  powerRight = 100;
  if (powerRight < -100) powerRight = -100;
}

//---------------------------------------------------------------------
// void CalcInterval(long cLoop)
//
// Calculate the interval time from one iteration of the loop to the next.
// Note that first time through, cLoop is 0, and has not gone through
// the body of the loop yet.  Use it to save the start time.
// After the first iteration, take the average time and convert it to
// seconds for use as interval time.
inline void CalcInterval(long cLoop)
{
  if (cLoop == 0) {
    // First time through, set an initial tInterval time and
    // record start time
    tInterval = 0.0055;
    tCalcStart = CurrentTick();
  } else {
    // Take average of number of times through the loop and
    // use for interval time.
    tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000.0);
  }
}

//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall.  The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle  This is the angle of the robot, it is the results of
//            integrating on the gyro value.
//            Units: degrees
// gyroSpeed  The value from the Gyro Sensor after offset subtracted
//            Units: degrees/second
// motorPos   This is the motor position used for balancing.
//            Note that this variable has two sources of input:
//             Change in motor position based on the sum of
//             MotorRotationCount of the two motors,
//            and,
//             forced movement based on user driving the robot.
//            Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
//            motor encoders.
//            Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
//     power = KGYROSPEED * gyro +
//             KGYROANGLE * gyroAngle +
//             KPOS       * motorPos +
//             KSPEED     * motorSpeed;
//
task taskBalance()
{
  Follows(main);

  float gyroSpeed, gyroAngle;
  float motorSpeed;

  int power, powerLeft, powerRight;
  long tMotorPosOK;
  long cLoop = 0;

  ClearScreen();
  TextOut(0, LCD_LINE1, "HiTechnic-HTWayC");
  TextOut(0, LCD_LINE4, "Balancing");

  tMotorPosOK = CurrentTick();

  // Reset the motors to make sure we start at a zero position
  ResetRotationCount(LEFT_MOTOR);
  ResetRotationCount(RIGHT_MOTOR);

  while(true) {
     CalcInterval(cLoop++);

     GetGyroData(gyroSpeed, gyroAngle);

     GetMotorData(motorSpeed, motorPos);

     // Apply the drive control value to the motor position to get robot
     // to move.
     motorPos -= motorControlDrive * tInterval;

     // This is the main balancing equation
     power = (KGYROSPEED * gyroSpeed +               // Deg/Sec from Gyro sensor
              KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
             KPOS       * motorPos +                 // From MotorRotaionCount of both motors
             KDRIVE     * motorControlDrive +        // To improve start/stop performance
             KSPEED     * motorSpeed;                // Motor speed in Deg/Sec

     if (abs(power) < 100)
       tMotorPosOK = CurrentTick();



     SteerControl(power, powerLeft, powerRight);

     // Apply the power values to the motors
     OnFwd(LEFT_MOTOR, powerLeft);
     OnFwd(RIGHT_MOTOR, powerRight);

     // Check if robot has fallen by detecting that motorPos is being limited
     // for an extended amount of time.
     if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
       break;
     }

     Wait(WAIT_TIME);
  }
  Off(MOTORS);

  TextOut(0, LCD_LINE4, "Oops... I fell");

  TextOut(0, LCD_LINE8, "tInt ms:");
  NumOut(6*8, LCD_LINE8, tInterval*1000);
}

/*
//=====================================================================
// taskControl
// This task runs in parallel to taskBalance.  This task monitors
// the IR Receiver and sets the global variables motorControlDrive
// and motorControlSteer.  Both of these values are in degrees/second.
//
task taskControl()
{
  Follows(main);

  char pfData[8];

  while(true) {
    ReadSensorHTIRReceiver(IR_RECEIVER, pfData);
    if (pfData[IR_LEFT] == -128) pfData[IR_LEFT] = 0;
    if (pfData[IR_RIGHT] == -128) pfData[IR_RIGHT] = 0;

    // Set control Drive and Steer.  These are in motor degree/second
    motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
    motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;


    Wait(CONTROL_WAIT);
  }
  // Wait to allow user time to see screen.
  Wait(10000);
}
*/