//Mobile robot (speed control with odometry)
//written by: Amirul Amin
//Date: Nov 2024

// Define motor driver pins
const int motor1In1 = 15;  // IN1 for Motor 1
const int motor1In2 = 2;  // IN2 for Motor 1
const int motor2In1 = 0;  // IN3 for Motor 2
const int motor2In2 = 4;  // IN4 for Motor 2
const int motor1PWM = 16;  // PWM for Motor 1 speed control
const int motor2PWM = 17;  // PWM for Motor 2 speed control

// setting PWM properties
const int freq = 1000;
const int resolution = 8;

// Encoder pins and variables
const int encoder1Pin = 33; // Encoder output for Motor 1
const int encoder2Pin = 32; // Encoder output for Motor 2
volatile int encoder1Count = 0;
volatile int encoder2Count = 0;
const int slots = 20;       // Number of slots in encoder disk

// Variables for speed, position, and heading calculations
float Pi = 3.142;
float wheelDiameter = 0.065;  // Wheel diameter in meters (adjust if needed)
float wheelBase = 0.014;       // Distance between wheels in meters (adjust if needed)
float motorSpeed1 = 0;        // Speed of Motor 1 (in m/s)
float motorSpeed2 = 0;        // Speed of Motor 2 (in m/s)
float motorRPM1 = 0;          // Speed of Motor 1 (in rpm)
float motorRPM2 = 0;          // Speed of Motor 2 (in rpm)
float posX = 0, posY = 0;     // Position coordinates of the robot
float heading = 0;            // Heading angle of the robot (in radians)
float headingDegrees = 0;     // Heading angle of the robot (in degrees)
unsigned long prevTime = 0;   // To calculate elapsed time

// Interrupt service routines for encoder counting
void IRAM_ATTR encoder1ISR() {
  encoder1Count++;
}

void IRAM_ATTR encoder2ISR() {
  encoder2Count++;
}

// Function to set motor direction and speed
void setMotorSpeed(int motor, int direction, int speed) {
  if (motor == 1) { // Motor 1
    digitalWrite(motor1In1, direction == 1 ? HIGH : LOW);
    digitalWrite(motor1In2, direction == 1 ? LOW : HIGH);
    ledcWrite(motor1PWM, speed);  // Channel 0 for motor1 PWM
  } 
  else if (motor == 2) { // Motor 2
    digitalWrite(motor2In1, direction == 1 ? HIGH : LOW);
    digitalWrite(motor2In2, direction == 1 ? LOW : HIGH);
    ledcWrite(motor2PWM, speed);  // Channel 1 for motor2 PWM
  }
}

// Function to calculate speed, position, and heading
void calculateOdometry() {
  // Calculate time elapsed
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - prevTime) / 1000.0; // seconds
  prevTime = currentTime;

  Serial.print("deltaTime =");
  Serial.println(deltaTime);

  if (deltaTime == 0){     // do not update odometry calculation to avoid inf error
    // Reset encoder counts
    encoder1Count = 0;
    encoder2Count = 0;
    return;
  }

  // Calculate wheel speeds (m/s)
  float distPerCount = (PI * wheelDiameter) / slots;
  motorSpeed1 = (encoder1Count * distPerCount) / deltaTime;
  motorSpeed2 = (encoder2Count * distPerCount) / deltaTime;

  // Calculate motor RPM
  float countsPerSecond1 = encoder1Count / deltaTime;
  float countsPerSecond2 = encoder2Count / deltaTime;

  // Assuming 'slots' represents counts per revolution
  motorRPM1 = (countsPerSecond1 * 60) / slots;
  motorRPM2 = (countsPerSecond2 * 60) / slots;

  // Reset encoder counts
  encoder1Count = 0;
  encoder2Count = 0;

  float angularVelocity = (motorSpeed1 - motorSpeed2) / wheelBase;
  Serial.print("angularVelocity =");
  Serial.println(angularVelocity);

  if (motorSpeed1 == 0 && motorSpeed2 == 0){
    return;
  }

  else if (motorSpeed1 != motorSpeed2){
    // Calculate angular velocity and ICC radius
    float ICC_radius = (wheelBase * (motorSpeed1 + motorSpeed2)) / (2 * (motorSpeed1 - motorSpeed2));
    Serial.print("ICC_radius =");
    Serial.println(ICC_radius);
    float deltaHeading = angularVelocity * deltaTime;
    Serial.print("deltaHeading =");
    Serial.println(deltaHeading);
    float preHeading = heading;
    Serial.print("preHeading =");
    Serial.println(preHeading);

    // Update robot's position and heading
    posX += (ICC_radius * cos(deltaHeading) * sin(preHeading)) + (ICC_radius * cos(preHeading) * sin(deltaHeading)) - (ICC_radius * sin(preHeading));
    posY += (ICC_radius * sin(deltaHeading) * sin(preHeading)) + (ICC_radius * cos(preHeading) * cos(deltaHeading)) + (ICC_radius * cos(preHeading));
    heading = preHeading + deltaHeading;
  }

  else if (motorSpeed1 == motorSpeed2) {
    // Calculate linear velocity
    float linearVelocity = (motorSpeed1 + motorSpeed2) / 2;
    Serial.print("linearVelocity =");
    Serial.println(linearVelocity);
    float preHeading = heading;
    Serial.print("preHeading =");
    Serial.println(preHeading);

    // Update robot's position and heading
    posX += linearVelocity * cos(preHeading) * deltaTime;
    posY += linearVelocity * sin(preHeading) * deltaTime;
    heading += 0;
  }
    
  // Normalize and convert the angle to the range of -180 t0 180
  headingDegrees = atan2(sin(heading), cos(heading)) * 180.0 / PI;  
  if (headingDegrees < 0) {
  headingDegrees += 360.0;  // Ensure positive value from 0 to 360
  }
  
  
}

void setup() {
  // Motor pins setup
  pinMode(motor1In1, OUTPUT);
  pinMode(motor1In2, OUTPUT);
  pinMode(motor2In1, OUTPUT);
  pinMode(motor2In2, OUTPUT);

  // configure PWM for motor speed control
  ledcAttach(motor1PWM, freq, resolution);
  ledcAttach(motor2PWM, freq, resolution);

  // Encoder pins setup
  pinMode(encoder1Pin, INPUT_PULLUP);
  pinMode(encoder2Pin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoder1Pin), encoder1ISR, RISING);
  attachInterrupt(digitalPinToInterrupt(encoder2Pin), encoder2ISR, RISING);

  Serial.begin(115200); // For debugging output
  prevTime = millis();  // Initialize time
}

void loop() {
  // Set motors to forward at 50% speed (adjust as needed)
  setMotorSpeed(1, 1, 128); // Motor 1 forward, 50% speed
  setMotorSpeed(2, 1, 128); // Motor 2 forward, 50% speed

  // Calculate odometry for position and heading
  calculateOdometry();

  // Print speed, position, and heading for debugging
  Serial.print("Speed (m/s): Motor1 = ");
  Serial.print(motorSpeed1);
  Serial.print(", Motor2 = ");
  Serial.println(motorSpeed2);

  Serial.print("Speed (rpm): Motor1 = ");
  Serial.print(motorRPM1);
  Serial.print(", Motor2 = ");
  Serial.println(motorRPM2);

  Serial.print("Position (m): X = ");
  Serial.print(posX);
  Serial.print(", Y = ");
  Serial.println(posY);

  Serial.print("Heading (rad): ");
  Serial.println(heading);

  Serial.print("Heading (degrees): ");
  Serial.println(headingDegrees);

  delay(100); // Adjust for desired loop rate
}