// TurnRate Sensor: read data from analog gyroscope, output heading on lcd

/* Connections:
 E   : Pin 3
 R/W : not connected
 R/S : Pin 4
 DB4 : Pin 6
 DB5 : Pin 7
 DB6 : Pin 8
 DB7 : Pin 9


 Turn-Rate Sensor: A0
*/
// include LCD functions:
#include <LiquidCrystal.h>

// define the LCD screen
LiquidCrystal lcd(4, 3, 6, 7, 8, 9);

/* global variables: */
// current value on A0
int analogValue;
int analogValue_RelaxedState;
long wholeRotationHeading = 1200000;
long heading;
int headingInDegrees;
long lastMeasurementMillis;

// initialization
void setup()
{
   // LCD has 4 lines with 20 chars
   lcd.begin(20, 4);
   lcd.print("system ready");

  delay(500);
  analogValue_RelaxedState = analogRead(A0);
}

void loop()
{
   // read the current analog value
   analogValue = analogRead(A0);
   if(abs(analogValue - analogValue_RelaxedState) < 10){
     analogValue = analogValue_RelaxedState;
   }
   int rotationRate = (analogValue - analogValue_RelaxedState);
   // calculate heading
   long millisSinceLastMeasurement = (millis() - lastMeasurementMillis);
   if(millisSinceLastMeasurement > 100){
     heading = ((heading + (millisSinceLastMeasurement * rotationRate)) % wholeRotationHeading);
     if(heading < 0){
       heading = (wholeRotationHeading - heading);
     }
     headingInDegrees = (heading / (wholeRotationHeading / 360));
     lastMeasurementMillis = millis();
   }
   // implement your code here:
   lcd.clear();
   lcd.print("Analog 0: ");
   lcd.print(analogValue);
   lcd.setCursor(0, 1);
   lcd.print("turn-rate: ");
   lcd.print(rotationRate);
   lcd.setCursor(0, 2);
   lcd.print("heading: ");
   lcd.print(heading);
   lcd.setCursor(0, 3);
   lcd.print("heading: ");
   lcd.print(headingInDegrees);
   lcd.print("deg");
   delay(40);
}


/* Usefull LCD functions:
set the current write position of the lcd to specific line and row:
  lcd.setCursor(row, line);

clear the lcd:
  lcd.clear();

write onto lcd, starting at current position:
  lcd.print("abc");
*/