//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
}
}