//BY Lynnate Jane Nazziwa & Kasirye Joshua
//Created on : 26th Oct 2024
//Embbedded Systems Assignment 1

#include <Arduino.h>  //For RTOS

//Right Motor Variables
const int LEFT_MOTOR_DIR1 = 32;
const int LEFT_MOTOR_PWM = 33;
//Right Motor Variable
const int RIGHT_MOTOR_DIR1 = 18;
const int RIGHT_MOTOR_PWM = 5;

// Define left and right encoder pins
#define LEFT_ENCODER_A 34
#define RIGHT_ENCODER_A 35

// Define Two IR sensor pins left and right sensors
#define LEFT_IR_SENSOR 22
#define RIGHT_IR_SENSOR 23

#define turn_left_LED 26
#define turn_right_LED 17

//Ultrsonic sensor (Echo:21 Trig:19)
#define TRIG_PIN 19
#define ECHO_PIN 21


///////////////////////////////////////////////// GLOBAL VARIABLES //////////////////////////////////////////
long duration; // Used by Ultrsonic sensor
int distance;  // Used by Ultrsonic sensor

// Speed control variables
float left_motor_speed = 0;
float right_motor_speed = 0;
// Line following variables
int base_speed = 140;  // Base speed of the motors
int turn_speed = 80;   //Turning speed
int stepsPerRevolution = 200;

// PID parameters
float Kp = 80.0;   // Proportional gain 40.5
float Ki = 0.0005;  // Integral gain
float Kd = 0.05;   // Derivative gain 0.005

// PID variables
float error = 0;
float prev_error = 0;
float integral = 0;
float derivative = 0;

// Position variables
float x_pos = 0.0;            // x-coordinate in meters
float y_pos = 0.0;            // y-coordinate in meters
float heading_angle = 0.0;    // Heading angle in radians
unsigned long prev_time = 0;  // Previous time for odometry update

// Variables for odometry
volatile long left_encoder_count = 0;
volatile long right_encoder_count = 0;
float wheel_diameter = 0.065;        // Diameter in meters
float wheel_base = 0.135;            // Distance between wheels in meters
float counts_per_revolution = 20.0;  // Encoder counts per wheel revolution for 20 slots only one output


// Interrupt service routines for both left and right encoders
void IRAM_ATTR leftEncoderISR() {
  left_encoder_count++;  // Increment count when an interrupt occurs
}

void IRAM_ATTR rightEncoderISR() {
  right_encoder_count++;
}


///////////////////////////////////////////////// SETUP FUNCTION //////////////////////////////////////////
void setup() {
  // put your setup code here, to run once:
  pinMode(LEFT_MOTOR_DIR1, OUTPUT);
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR1, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);

  // Encoder pin setup
  pinMode(LEFT_ENCODER_A, INPUT);
  pinMode(RIGHT_ENCODER_A, INPUT);

  // IR sensor pin setup
  pinMode(LEFT_IR_SENSOR, INPUT);
  pinMode(RIGHT_IR_SENSOR, INPUT);

  //Ultrasonic sensor
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // MOTOR LEDs pin setup
  pinMode(turn_left_LED, OUTPUT);
  pinMode(turn_right_LED, OUTPUT);


  // Attach interrupts to encoder pins
  attachInterrupt(digitalPinToInterrupt(LEFT_ENCODER_A), leftEncoderISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RIGHT_ENCODER_A), rightEncoderISR, CHANGE);

  Serial.begin(115200);
  Serial.println("Hello, This is Our Embedded Systems Project 1!");

  //Initiaize robot motors to stop
  controlMotors(0, 0);

  // Create a FreeRTOS task for line following
  xTaskCreate(
    line_following,      // Task function
    "Line Follow Task",  // Task name
    1024,                // Stack size (in bytes)
    NULL,                // Task parameter
    3,                   // Task priority
    NULL                 // Task handle
  );

  digitalWrite(turn_right_LED, LOW);
  digitalWrite(turn_left_LED, LOW);
  prev_time = millis(); // Initialize previous time
}


///////////////////////////////////////////////// LOOP FUNCTION //////////////////////////////////////////
void loop() {
  //To run the robot in only one direction, hence no
  /*
  controlMotors(5, 5);
  delay(5000);
  controlMotors(-5, -5);
  delay(5000);
  controlMotors(5, 0);
  delay(5000);
  controlMotors(0, 5);
  delay(5000);
  controlMotors(0, 0);
  delay(5000);
  */

  // Calculate robot odometry information
  Calculate_odometry();

  // Print odometry and position data
  Serial.print("X: "); Serial.print(x_pos); Serial.print(" m, Y: "); Serial.print(y_pos);
  Serial.print(" m, Heading: "); Serial.print(heading_angle * (180 / PI)); Serial.println(" degrees");
  delay(500);
}



///////////////////////////////////////////////// OTHER FUNCTIONS //////////////////////////////////////////

//Control left and right motors turning and direction based on speed values
//Improved controlMotors function with smooth adjustments
void controlMotors(int left_speed, int right_speed) {
  // Clamp speed values to valid range (0-255 for PWM)
  left_speed = constrain(left_speed, -255, 255); //Making sure all speeds don't exit the 8 bit PWM output
  right_speed = constrain(right_speed, -255, 255);

  // Control left motor direction
  if (left_speed > 0) {
    digitalWrite(LEFT_MOTOR_DIR1, HIGH); //CW
  } else {
    digitalWrite(LEFT_MOTOR_DIR1, LOW); //CCW
  }
  analogWrite(LEFT_MOTOR_PWM, abs(left_speed));

  // Control right motor direction
  if (right_speed > 0) {
    digitalWrite(RIGHT_MOTOR_DIR1, HIGH); //CW
  } else {
    digitalWrite(RIGHT_MOTOR_DIR1, LOW); //CCW
  }
  analogWrite(RIGHT_MOTOR_PWM, abs(right_speed));
}


//Function to calculate robot odometry position, speed and heading angle
void Calculate_odometry() {
  unsigned long current_time = millis();
  float dt = (current_time - prev_time) / 1000.0; // Time in seconds
  prev_time = current_time;

  float left_wheel_distance = (left_encoder_count / counts_per_revolution) * (PI * wheel_diameter);
  float right_wheel_distance = (right_encoder_count / counts_per_revolution) * (PI * wheel_diameter);

  float linear_distance = (left_wheel_distance + right_wheel_distance) / 2;
  float angular_change = (right_wheel_distance - left_wheel_distance) / wheel_base;

  // Update heading angle
  heading_angle += angular_change;

  // Update x and y coordinates
  x_pos += linear_distance * cos(heading_angle);
  y_pos += linear_distance * sin(heading_angle);
}

// Calculate obstance distance for avoidance
float get_obstacle_distance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;  //343 meters per second to 0.034cm/us

  return distance;
}

//Line following task based on two IR sensors
void line_following(void* parameter) {
  while (true) {
    // Read IR sensor values
    bool left_ir = digitalRead(LEFT_IR_SENSOR);
    bool right_ir = digitalRead(RIGHT_IR_SENSOR);

    // Calculate error based on sensor readings
    if (!left_ir && right_ir) {
      error = -1;  // Line is to the right
      digitalWrite(turn_right_LED, HIGH);
    } else if (left_ir && !right_ir) {
      error = 1;  // Line is to the left // Left sensor on line, turn left
      digitalWrite(turn_left_LED, HIGH);
    } else {
      error = 0;  // Line is centered
      digitalWrite(turn_right_LED, LOW);
      digitalWrite(turn_left_LED, LOW);
      controlMotors(0, 0);

    }

    // PID calculations
    integral += error;
    derivative = error - prev_error;
    prev_error = error;

    // Calculate correction
    float correction = (Kp * error) + (Ki * integral) + (Kd * derivative);

    // Adjust motor speeds
    int left_motor_speed = base_speed - correction;
    int right_motor_speed = base_speed + correction;

    // Control motors with PID-adjusted speed
    controlMotors(left_motor_speed, right_motor_speed);

    //get obstace distance
    while (get_obstacle_distance() <= 10) {  //10cm, stop robot from moving
      controlMotors(0, 0);
    }

    // Small delay to prevent task overload (optional)
    vTaskDelay(1 / portTICK_PERIOD_MS);  // Delay for 10ms
  }
}

$abcdeabcde151015202530fghijfghij
A4988
A4988