lab3-activity05-group14 (4).ino

/**
 * @file sharp-sensor.ino
 * @author Joshua Marshall (joshua.marshall@queensu.ca)
 * @brief Arduino program to read proximity data from a Sharp GP2Y0A21YK.
 * @version 1.0
 * @date 2021-11-16
 *
 * @copyright Copyright (c) 2021
 *
 */

#include <math.h>
// Arduino analog input pin to which the Sharp sensor is connected
const byte SHARP_PIN = A5;
const byte SHARP_PINL = A3;
const byte SHARP_PINR= A4;

// Variable to store the proximity measurement
int sharp_val = 0;
int sharp_valL = 0;
int sharp_valR = 0;
double distance = 0;
double distanceL = 0;
double distanceR= 0;

//movement pins
// Motor driver PWM pins
const byte E1 = 6;
const byte E2 = 5;

// Motor driver direction pins
const byte M1 = 7;
const byte M2 = 4;

// Left wheel encoder digital pins
const byte SIGNAL_AL = 8;
const byte SIGNAL_BL = 9;

// Right wheel encoder digital pins
const byte SIGNAL_AR = 10;
const byte SIGNAL_BR = 11;

/* USEFUL CONSTANTS */

// Encoder ticks per (motor) revolution (TPR)
const int TPR = 3000;

// Wheel radius [m]
const double RHO = 0.0625;

// Vehicle track [m]
const double ELL = 0.2775;

// Sampling interval for measurements in milliseconds
const int T = 100;

// Controller gains (use the same values for both wheels)
const double KP = 200.0; // Proportional gain
const double KI = 100.0; // Integral gain

/* VARIABLE DECLARATIONS */

// Motor PWM command variables [0-255]
short u_L = 0;
short u_R = 0;

// Counter to keep track of encoder ticks [integer]
volatile long encoder_ticks_L = 0;
volatile long encoder_ticks_R = 0;

// Variables to store estimated angular rate of left wheel [rad/s]
double omega_L = 0.0;
double omega_R = 0.0;

// Variables to store estimate wheel speeds [m/s]
double v_L = 0.0;
double v_R = 0.0;

// Variables to store vehicle speed and turning rate
double v = 0.0;     // [m/s]
double omega = 0.0; // [rad/s]

// Variables to store desired vehicle speed and turning rate
double v_d = 0.0;     // [m/s]
double omega_d = 0.0; // [rad/s]

// Variable to store desired wheel speeds [m/s]
double v_Ld = 0.0;
double v_Rd = 0.0;

// Counters for milliseconds during interval
long t_now = 0;
long t_last = 0;

// Variables to store errors for controller
double e_L = 0.0;
double e_R = 0.0;
double e_Lint = 0.0;
double e_Rint = 0.0;


// track constant
const double L = 0.2775;
/* HELPER FUNCTIONS */

// This function is called when SIGNAL_AL (left encoder) goes HIGH
void decodeEncoderTicks_L()
{
    if (digitalRead(SIGNAL_BL) == LOW)
    {
        // SIGNAL_A leads SIGNAL_B, so count one way
        encoder_ticks_L--;
    }
    else
    {
        // SIGNAL_B leads SIGNAL_A, so count the other way
        encoder_ticks_L++;
    }
}

// This function is called when SIGNAL_AR (right encoder) goes HIGH
void decodeEncoderTicks_R()
{
    if (digitalRead(SIGNAL_BR) == LOW)
    {
        // SIGNAL_A leads SIGNAL_B, so count one way
        encoder_ticks_R++;
    }
    else
    {
        // SIGNAL_B leads SIGNAL_A, so count the other way
        encoder_ticks_R--;
    }
}

// Compute the wheel rate from elapsed time and encoder ticks [rad/s]
double compute_wheel_rate(long encoder_ticks, double delta_t)
{
    double omega;
    omega = 2.0 * PI * ((double)encoder_ticks / (double)TPR) * 1000.0 / delta_t;
    return omega;
}

// Compute wheel speed [m/s]
double compute_wheel_speed(double omega_wheel)
{
    double v_wheel;
    v_wheel = omega_wheel * RHO;
    return v_wheel;
}

// Compute vehicle speed [m/s]
double compute_vehicle_speed(double v_L, double v_R)
{
  double v;
  v = 0.5 * (v_L + v_R);
  return v;
}

// Compute vehicle turning rate [rad/s]
double compute_vehicle_rate(double v_L, double v_R)
{
  double omega;
  omega = (1.0 / ELL) * (v_R - v_L);
  return omega;
}

// Compute v_L from v and omega
double compute_L_wheel_speed(double v, double omega)
{
  double v_Ld; 
  v_Ld = ((v*2)-(ELL*omega))/2; 
    return v_Ld; 
}

// Compute v_R from v and omega
double compute_R_wheel_speed(double v, double omega)
{
  double v_Rd;
  v_Rd = ((2*v)+(ELL*omega))/2; 
    return v_Rd; 
}

// Wheel speed PI controller function
short PI_controller(double e_now, double e_int, double k_P, double k_I)
{
    short u;
    u = (short)(k_P * e_now + k_I * e_int);
    if (u > 255)
    {
        u = 255;
    }
    else if (u < -255)
    {
        u = -255;
    }
    return u;
}
//setup
void setup()
{
    // Open the serial port at 9600 bps
    Serial.begin(9600);
    // Set the pin modes for the motor driver
    pinMode(M1, OUTPUT);
    pinMode(M2, OUTPUT);

    // Set the pin modes for the encoders
    pinMode(SIGNAL_AL, INPUT);
    pinMode(SIGNAL_BL, INPUT);
    pinMode(SIGNAL_AR, INPUT);
    pinMode(SIGNAL_BR, INPUT);
    
    // Every time the pin goes high, this is a pulse
    attachInterrupt(digitalPinToInterrupt(SIGNAL_AL), decodeEncoderTicks_L,
                    RISING);
    attachInterrupt(digitalPinToInterrupt(SIGNAL_AR), decodeEncoderTicks_R,
                    RISING);

    // Print a message
    Serial.print("Program initialized.");
    Serial.print("\n");
}

void loop()
{
  //driving information 

    // Read the sensor output (0-1023, or 10 bits)
    sharp_val = analogRead(SHARP_PIN);
    sharp_valL = analogRead(SHARP_PINL);
    sharp_valR = analogRead(SHARP_PINR);
    distance = (5500/sharp_val);
    distanceL = (5500/sharp_valL); 
    distanceR = (5500/sharp_valR);
    // Print the value to the Serial Monitor
    //Serial.print(sharp_val);
    //Serial.println(distance);
    //Serial.println(distanceL);
    //Serial.println(distanceR);
    if(distance <= 22 ){
      //stop rover
    }
    // Get the elapsed time [ms]
    t_now = millis();

    if (t_now - t_last >= T)
    {

        // Set the desired vehicle speed and turning rate
        v_d = 0.1;     // [m/s]
        omega_d = 0.0; // [rad/s]

        // Estimate the rotational speed of each wheel [rad/s]
        omega_L = compute_wheel_rate(encoder_ticks_L, (double)(t_now - t_last));
        omega_R = compute_wheel_rate(encoder_ticks_R, (double)(t_now - t_last));

        // Compute the speed of each wheel [m/s]
        v_L = compute_wheel_speed(omega_L);
        v_R = compute_wheel_speed(omega_R);

        // Compute the speed of the vehicle [m/s]
        v = compute_vehicle_speed(v_L, v_R);

        // Compute the turning rate of the vehicle [rad/s]
        omega = compute_vehicle_rate(v_L, v_R);

        // Record the current time [ms]
        t_last = t_now;

        // Reset the encoder ticks counter
        encoder_ticks_L = 0;
        encoder_ticks_R = 0;

        // Compute the desired wheel speeds from v_d and omega_d
        v_Ld = compute_L_wheel_speed(v_d, omega_d);
        v_Rd = compute_R_wheel_speed(v_d, omega_d);

        // Compute errors
        e_L = v_Ld - v_L;
        e_R = v_Rd - v_R;

        // Integrate errors with anti-windup
        if (abs(u_L) < 255)
        {
            e_Lint += e_L;
        }
        if (abs(u_R) < 255)
        {
            e_Rint += e_R;
        }

        // Compute control signals using PI controller
        u_L = PI_controller(e_L, e_Lint, KP, KI);
        u_R = PI_controller(e_R, e_Rint, KP, KI);
      // Write to the output pins
        if (u_L < 0)
        {
            digitalWrite(M1, HIGH); // Drive backward (left wheels)
            analogWrite(E1, -u_L);  // Write left motors command
        }
        else
        {
            digitalWrite(M1, LOW); // Drive forward (left wheels)
            analogWrite(E1, u_L);  // Write left motors command
        }

        if (u_R < 0)
        {
            digitalWrite(M2, LOW); // Drive backward (right wheels)
            analogWrite(E2, -u_R); // Write right motors command
        }
        else
        {
            digitalWrite(M2, HIGH); // Drive forward (right wheels)
            analogWrite(E2, u_R);   // Write right motors command
        }
    }
    // Delay for a bit before reading the sensor again
    delay(500);
}