// Zoe_Odometer = Pgm to read pulses from two hall effect sensors pointing at
// sprocket teeth for Zoe's main drive.
// 9-21-21 Added speed measuring
// 9-07-25 Added current measuring Odom_Speed_Cur.ino

// library code:
#include            // Need WIRE to talk to serial port
#include   // Need LiquidCrystal to talk to LCD display

LiquidCrystal lcd(12, 11, 4, 5, 6, 7);  // Define LCD display pins RS,E,D4,D5,D6,D7

// Pins:
const int PinLED = 13;
const int PinReset = 8;
const int LeftAnalogPin = A6;
const int RightAnalogPin = A5;

// Varz:
volatile unsigned long CountLeft = 0;
volatile unsigned long CountRight = 0;
bool LEDOn = true;
bool PinResetValue = HIGH;

// Varz for Speed Measuring
volatile unsigned long PreviousLeft = 0;
volatile unsigned long PreviousRight = 0;
volatile unsigned long LatestLeft = 0;
volatile unsigned long LatestRight = 0;
unsigned long PPSLeft = 0;
unsigned long PPSRight = 0;

// Varz for current sensing
volatile int LeftIsensor;  // 0-1023
volatile int RightIsensor;

int LeftCurrent;
int RightCurrent;
int NullRight;
int NullLeft;

// The setup function runs once when you press reset or power on the board
void setup() {
  pinMode(PinReset, INPUT_PULLUP);

  // Set up the serial port
  Serial.begin(9600);
  Wire.begin();

  //  Set up LCD as 16x2 type
  lcd.begin(16, 2);

  lcd.setCursor(0, 0);
  lcd.print("Zoe The Robot");

  lcd.setCursor(0, 1);
  lcd.print("Odometry");

  delay(2000);
  lcd.clear();

  pinMode(PinLED, OUTPUT);  // Built in LED

  Serial.println("* Hard Reset");

  // Choices are:  RISING ,  LOW , FALLING , CHANGE
  attachInterrupt(digitalPinToInterrupt(2), IncrRightISR, RISING);  //  function for creating external interrupts at pin2 on Rising (LOW to HIGH)
  attachInterrupt(digitalPinToInterrupt(3), IncrLeftISR, RISING);   //  function for creating external interrupts at pin3 on Rising (LOW to HIGH)

  // Get Null Current reading - When no motion
  delay(1000);
  NullLeft = Round(analogRead(LeftAnalogPin));
  delay(1000);
  NullRight = Round(analogRead(RightAnalogPin));


  // Show the nulls for 5 seconds
  lcd.setCursor(0, 0);  // top line
  lcd.print("NullLeft =  ");
  lcd.print(NullLeft);
  //
  lcd.setCursor(0, 1);  // second line
  lcd.print("NullRight = ");
  lcd.print(NullRight);
  //
  delay(2000);
}


// The loop function runs over and over again forever
void loop() {

  //  Main Loop
  LatestRight = CountRight;
  LatestLeft = CountLeft;

  // Calculate pulses per second
  PPSRight = (LatestRight - PreviousRight) / 2;
  PPSLeft = (LatestLeft - PreviousLeft) / 2;


  // Get current reading
  LeftIsensor = Round(analogRead(LeftAnalogPin));
  RightIsensor = Round(analogRead(RightAnalogPin));

  // Fix the current readings
  LeftCurrent = LeftIsensor - NullLeft;
  RightCurrent = RightIsensor - NullRight;

  if (abs(LeftCurrent) < 10) {
    LeftCurrent = 0;
  }

  if (abs(RightCurrent) < 10) {
    RightCurrent = 0;
  }


  ////////////////////////////////////////////////
  // start with blank slate
  lcd.clear();


  // Display Right Count on LCD
  lcd.setCursor(0, 0);  // top line
  lcd.print("R ");
  lcd.print(LatestRight);


  // Display Right Current on LCD
  lcd.setCursor(9, 0);  // top line
  lcd.print(RightCurrent);


  // Display Right Speed on LCD
  lcd.setCursor(14, 0);
  lcd.print("  ");
  lcd.setCursor(14, 0);
  lcd.print(PPSRight);

  ////////////////////////////////////////////

  // Display left Count on LCD
  lcd.setCursor(0, 1);  // second line
  lcd.print("L ");
  lcd.print(LatestLeft);


  // Display left Current on LCD
  lcd.setCursor(9, 1);  // second line
  lcd.print(LeftCurrent);


  // Display Left Speed on LCD
  lcd.setCursor(14, 1);
  lcd.print("  ");
  lcd.setCursor(14, 1);
  lcd.print(PPSLeft);

  /////////////////////////////////////////


  // Send Counts to serial port
  Serial.print("R:");  // pulse count right
  Serial.print(LatestRight);

  Serial.print(" SR:");
  Serial.print(PPSRight);

  Serial.print(" IR:");  // IR = Current Right
  Serial.print(RightCurrent);


  ////////////////////////////////////////


  Serial.print(" L:");  // pulse count left
  Serial.print(LatestLeft);

  Serial.print(" SL:");  // speed left
  Serial.print(PPSLeft);

  Serial.print(" IL:");  // IL = Current Left
  Serial.print(LeftCurrent);




  //////  for testing /////////////////
  // Serial.print("    >>>>>>    ");

  // Serial.print("NullRight: ");
  // Serial.print(NullRight);

  // Serial.print("   NullLeft: ");
  // Serial.print(NullLeft);

  // Line Feed
  Serial.println();

  // Supposed to give me current, in amps
  //  (0.0732 * LeftIsensor)-37.2
  //  (0.0732 * RightIsensor)-37.2


  delay(987);  // CHG'D FROM 2000  987

  PreviousLeft = LatestLeft;
  PreviousRight = LatestRight;

  // Toggle LED
  LEDOn = !LEDOn;
  if (LEDOn) {
    digitalWrite(PinLED, HIGH);
  } else {
    digitalWrite(PinLED, LOW);
  }

  // Check an input pin for counter reset signal
  PinResetValue = digitalRead(PinReset);  // read the input pin

  // Call CounterReset if PinReset goes low
  if (PinResetValue == LOW) {
    CounterReset();
    Serial.println("* Pin Reset");
  }
}
// End of loop




// This code executes if data is recvd by serial port
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();

    // Call CounterReset if R received
    if (inChar == 'R') {
      Serial.println("* Serial Reset");
      CounterReset();
    }
  }
}

void IncrRightISR() {
  CountRight++;
}

void IncrLeftISR() {
  CountLeft++;
}

void CounterReset() {
  CountRight = 0;
  CountLeft = 0;
  PreviousLeft = 0;
  PreviousRight = 0;
  lcd.clear();
}

int Round(int In) {
  int Result;
  int Fudge = 1;
  Result = (In / Fudge) * Fudge;
  return Result;
}