Elisa3 Robot PIC control

This is me testing out some software I wrote to control this dinky little Elisa3 robot I’ve been loaned by the university. This makes use of a PID controller, which takes the distance from the white card and drives the motors forward or back to keep the robot as close to 15mm away from the card as it can.

The motors here are a bit sticky so the robot doesn’t go straight. But the program works and that was the point of the exercise.

Paws assisting on the left.

About Elisa3

Arduino Code

Elisa3 features an Arduino compatible Atmel 2560 microcontroller. I've had to add libraries and boards information manually to the Arduino IDE to get this working in Arduino 1.8.11. All the details about that are on the Gctronic wiki

//Elisa3 PID Control

volatile int SensorReading = 0;
const int DesiredDistance = 10;
volatile int DistanceError = 0;
volatile int PreviousError = 0;
volatile float integral = 0;
volatile float diff = 0;
volatile float output = 0;
const float saturation_max = 15, saturation_min = -15;

float Kp = 1, Ki = 0, Kd = 0;


//****************************************
//SETUP CODE*******************************
//****************************************

void setup()
{
  DDRA |= (1<<PA0); //Set the IR-LED at front as an output
  SetupTimer1();
  Serial.begin(9600);
}


//****************************************
//MAIN CODE*******************************
//****************************************
void loop()
{
  right_motor(output);
  left_motor(output);
  Serial.print("PID output: ");
  Serial.print(output);
  Serial.print(" distance: ");
  Serial.println(SensorReading);

}
//MAIN CODE ENDS**************************



//****************************************
void SetupTimer1()  //64bit prescaler timer triggering every 50ms
{
  TCCR1A = 0;
  TCCR1B = (1<WGM12) | (1<<CS11) | (1<<CS10);   //clear and compare
  TCNT1 = 0;
  OCR1A = 6249;
  TIMSK1 |= (1<<OCIE1A);
  sei();
}

//****************************************
ISR (TIMER1_COMPA_vect) //interrupt service routine
{
  //Read the sensor
  TurnProxLedOn();
  delay(5);
  SensorReading = 0.02033 * analogRead(0) - 0.888; //gives value in mm
  TurnProxLedOff();

  //Calculate the new values for PID
  DistanceError = SensorReading - DesiredDistance;  //How far off we are (DistanceError is P)

  integral = integral + (DistanceError * 0.05);     //(intergral is I)
  diff = ((DistanceError - PreviousError) / 0.05);  //(diff is D)

  output = (Kp *  DistanceError) + (Ki * integral) + (Kd * diff);
  //output = (Kp *  DistanceError);

  PreviousError = DistanceError;  //save this error for the next timer

  //Prevent Saturation
  if (output > saturation_max)
    {
      output = saturation_max;
    }
  else if (output < saturation_min)
    {
      output = saturation_min;
    }
  
  
}


//****************************************
void TurnProxLedOn()
{
  PORTA |= (1<<PB0);
}
//****************************************
void TurnProxLedOff()
{
  PORTA &= ~(1<<PB0);
}
//****************************************



//****************************************
//DRIVE DEFINITIONS***********************
//****************************************
void right_motor(float right_speed)
{
  if (right_speed >0) //move motor forward
    {
      analogWrite(5, right_speed * 2.6);
      analogWrite(2,0);
    }
    else //move motor backward
    {
      analogWrite(2, -right_speed * 2.6);
      analogWrite(5,0);
    }
}
void left_motor(float left_speed)
{
  if (left_speed >0) //move motor forward
    {
      analogWrite(6, left_speed * 2.6);
      analogWrite(7,0);
    }
    else //move motor backard
    {
      analogWrite(7, -left_speed * 2.6);
      analogWrite(6,0);
    }
}