#include "NXCDefs.h"

#define GYRO_PORT S1        //Gyro on port 1
#define GYRO_Offset 620       //Offset value for the Gyro
#define PERIOD 20   		//Update frequency
#define PERIOD_Gyro 10		//Update frequency for the Gyro
#define OFFSET_SAMPLES 100   // Number of samples for offset

// Constants for power
// Observe that all variables are multiplied with 100 to compensate for lack of decimal.
#define L1                     30
#define L2                     30
#define L3                     30
#define L4                     5
#define L5                     30
#define Scale                  40

long Gyro_value;
long drift=-35;             //A good estimation of the drift for my sensor
long Gyro_angle=0;
long time_g;

int power,count,time;

byte theUF;                                     // Update Flags
byte theOM = OUT_MODE_MOTORON+OUT_MODE_BRAKE;   // Out Mode
byte theRM = OUT_REGMODE_IDLE+OUT_REGMODE_SYNC; // Reg Mode
byte theRS = OUT_RUNSTATE_RUNNING;              // Run State

// Function to read the sensor value and calculate the drift.

void calibrateSensor() {
    TextOut(12, LCD_LINE3, "Calibrating!");
     drift=0;
      SetSensorHTGyro(GYRO_PORT);
     for (int i=0; i<OFFSET_SAMPLES ;i++) {
          drift=drift+SensorHTGyro(GYRO_PORT)-GYRO_Offset;
          Wait(10);
     }
     drift=drift/OFFSET_SAMPLES;
     ClearScreen();
     TextOut(10, LCD_LINE3, "drift is:");
     NumOut(46, LCD_LINE5, drift);
     Wait(3000);
     ClearScreen();

}

//Task for reading gyro value
task gyro() {
	while (true) {
	time_g=CurrentTick();   // millisecond
	Gyro_value = SensorHTGyro(GYRO_PORT)-GYRO_Offset-drift;

	// Integration of gyro value to create angle

	Gyro_angle = Gyro_angle+Gyro_value*PERIOD_Gyro/1000;
	
	 	     ClearScreen();
     TextOut(10, LCD_LINE3, "gyro angle");
     NumOut(46, LCD_LINE5, Gyro_angle);
     Wait(10);
     ClearScreen();
	
	time_g=PERIOD_Gyro-CurrentTick()+time_g ;

	Wait(time_g);
}
}

//Task that stabilizes the pendulum

task balance() {

     while (true) {

         time=CurrentTick();
 	   power=(-Gyro_angle*L1-Gyro_value*L2)/Scale;
         // With position control

         	     ClearScreen();
    // TextOut(10, LCD_LINE3, "power");
    // NumOut(46, LCD_LINE5, power);
     Wait(300);
     ClearScreen();

   	   if (power > 50) {
             power = 50;
         } else if (power < -50) {
             power = -50;
         }
         SetOutput(OUT_AC, UpdateFlags, theUF, Power, power);

         time=PERIOD-CurrentTick()+time;
         Wait(time);
     }
}

task main() {

     theUF = UF_UPDATE_SPEED+UF_UPDATE_MODE;
     SetOutput(OUT_AC, OutputMode, theOM, RegMode, theRM, RunState, theRS, UpdateFlags, theUF, Power, power);
     theUF = UF_UPDATE_SPEED;
     calibrateSensor(); //Calibrate the sensor. Comment away if you want to use the predefined value.
     Precedes(balance,gyro);

}