Arduino and Matlab

I wanted to use Matlab's optimisation tool to optimise the sweep timings of the infra red sensor. To do this required the Arduino and Matlab have to communicate. Now Matlab has a toolbox for this, but I wanted my Arduino code as unchanged as possible and to run as fast as it would in normal operating conditions. So I decided to get Matlab to only talk to the Arduino via the USB cable, not program it. The Matlab code I used is this

function y=scan_results(x)

try
 
    dt_servo=x(1);
    dt_ir=x(2);
    n=x(3);
    resolution=20;
 
    %open serial port
    s = serial('COM4','BaudRate',115200);
    set(s,'DataBits', 8);
    set(s,'StopBits', 1);
    set(s,'Timeout', 60);
    fopen(s);
 
    %Read a line of data
    line=fgetl(s);
    fprintf('%s\n',line);
 
    %send the dt_servo value
    fwrite(s,int8(dt_servo));
     

    %Read a line of data

    line=fgetl(s);

    fprintf('%s\n',line);

 
    %send the dt_ir value
    fwrite(s,int8(dt_ir));
    

    %Read a line of data

    line=fgetl(s);

    fprintf('%s\n',line);

 
    %send the n value
    fwrite(s,int8(n));
     

    %Read a line of data

    line=fgetl(s);

    fprintf('%s\n',line);

     

    %Read a line of data

    line=fgetl(s);

    fprintf('%s\n',line);

 
    %Read data from Arduino
    angle=zeros(3,resolution+1);
    old_map=zeros(3,resolution+1);
    new_map=zeros(3,resolution+1);
    scan_right=zeros(3,resolution+1);
    scan_left=zeros(3,resolution+1);
    for i=1:3
        %Read angle data
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
 
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
        angle(i,:)=str2num(line);
     
        
        %Read old_map data
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
     
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
        old_map(i,:)=str2num(line);  
     
     
        %Read new_map data
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
     
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
        new_map(i,:)=str2num(line);
 
     
        %Read scan_right data
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
     
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
        scan_right(i,:)=str2num(line);
 
     
        %Read scan_left data
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
     
        %Read a line of data
        line=fgetl(s);
%         fprintf('%s\n',line);
        scan_left(i,:)=str2num(line);
     
%         figure(i)
%         polar((angle(i,:)/180)*pi,scan_right(i,:),'b')
%         hold on
%         polar((angle(i,:)/180)*pi,scan_left(i,:),'r')
%         hold off      
 
        y=0;
        for j=1:resolution+1
            y=y+sqrt((scan_right(i,j)-scan_left(i,j))^2); %objective function
        end
     
    end
 
 
    y=y+(dt_servo+dt_ir+n)*1; %objective function

catch
 
    %close serial port
    fclose(s);
    delete(s);
    clear s;

end

try
%close serial port
fclose(s);
delete(s);
clear s;

catch
 
end

fprintf('Results Obtained\n\n');

This function that opens a serial port to the Arduino with the same baud rate as the Serial.begin command on the Arduino. The databits and stopbits also need to be defined for it to work. It all needs to be in a try catch mechanism, so that the serial port is always closed. If the port isn't closed then you have to exit Matlab and reopen it. It was written to allow a Matlab optimiser to call it and it returns the value of my objective function. The rest of the program makes more sense when you can see what the Arduino code is doing.

#include <Servo.h>

#define resolution 20     //larger number means more accurate, but also more ram used
#define totalangle 100    //total angle that it turns 

#define scannercentre 79  //the scanner servo centre location
#define scannermin 3      //the pwm value to give an angle of 0 degrees
#define scannermax 152    //the pwm value to give an angle of 180 degrees

//define servos
Servo scanner;            // create servo object to control the sweeping servo 

//variables
int res2=resolution*0.5;
boolean initial=true;
int i,j,k;
int dt_servo=10000;           //delay for scanner servo to move to position in ms
int dt_ir=47;              //delay for the ir sensor to refresh in ms
int n=10;                  //number of readings to average from ir sensor
int reading;       // used for taking the averages
float dist;        // used for calculating the distance in cm

int new_map[resolution+offset+1]; //store the distances from the ir sensor
int old_map[resolution+offset+1]; //store the distances from the ir sensor
int scan_left[resolution+offset+1]; //store the distances from the ir sensor
int scan_right[resolution+offset+1]; //store the distances from the ir sensor
int angle[resolution+1];      //stores the angles at which data was taken

boolean run=false;

void setup()
{
  scanner.attach(6);    // attaches the servo on pin 6 to the servo object 

  Serial.begin(115200); // sets the speed of the serial connection to the computer

  scanner.write(scannercentre);    // sets the scanner servo to the centre

  analogReference(EXTERNAL);  // Set the ADC reference voltage to external (only use if using 3V, read this first)

  //calculate the angles to be used
  for(i=0;i<res2;i++) { //right sweep
    angle[i]=90-((res2-i)*(totalangle/resolution));
  }
  angle[res2]=90;  //centre
  for(i=res2+1;i<resolution+1;i++) { //left sweep
    angle[i]=90+((i-res2)*(totalangle/resolution));
  }

  //for debugging...
//  for(i=0;i<resolution+1;i++) {
//    Serial.print(i);
//    Serial.print(" ");
//    Serial.println(angle[i]);
//  }

  pinMode(13,OUTPUT);

  digitalWrite(13,HIGH);
  delay(1000);
  digitalWrite(13,LOW);
  delay(1000);

}

// This runs continuously on the arduino
void loop() {

  if(run==false) {
    Serial.println("Waiting for inputs");
    dt_servo=Serial.read();
    while(dt_servo==-1) {
      dt_servo=Serial.read();
      delay(100);
    }
    Serial.print("dt_servo = ");
    Serial.println(dt_servo);
 
    dt_ir=Serial.read();
    while(dt_ir==-1) {
      dt_ir=Serial.read();
      delay(100);
    }
    Serial.print("dt_ir = ");
    Serial.println(dt_ir);
 
    n=Serial.read();
    while(n==-1) {
      n=Serial.read();
      delay(100);
    }
    Serial.print("n = ");
    Serial.println(n);
 
    Serial.println("Done Reading");
    digitalWrite(13,HIGH);
    run=true;
  }
  else {
    for(k=0;k<3;k++) {
      for(i=0;i<(resolution*2)+2;i++) {
     
        //Look
        look();     
      }
   
      //print maps
      Serial.println("Angle:");
      for(i=0;i<resolution+1;i++) {
        Serial.print(angle[i]);
        if(i!=resolution) {
          Serial.print(" ");
        }
        else {
          Serial.println(" ");
        }
      }
      Serial.println("Old Map:");
      for(i=0;i<resolution+1;i++) {
        Serial.print(old_map[i]);

        if(i!=resolution) {

          Serial.print(" ");

        }

        else {

          Serial.println(" ");

        }

      }

      Serial.println("New Map:");
      for(i=0;i<resolution+1;i++) {
        Serial.print(new_map[i]);

        if(i!=resolution) {

          Serial.print(" ");

        }

        else {

          Serial.println(" ");

        }

      }

      Serial.println("Scan Right:");
      for(i=0;i<resolution+1;i++) {
        Serial.print(scan_right[i]);

        if(i!=resolution) {

          Serial.print(" ");

        }

        else {

          Serial.println(" ");

        }

      }

      Serial.println("Scan Left:");
      for(i=0;i<resolution+1;i++) {
        Serial.print(scan_left[i]);

        if(i!=resolution) {

          Serial.print(" ");

        }

        else {

          Serial.println(" ");

        }

      }

   
    }
    run=false;
    digitalWrite(13,LOW);
  }
}

void look() {

  if(i==0) { //read front
    //move servo to position
    scanner.write(mapscanner(angle[res2]));
    delay(dt_servo);
    take_reading();
    //update map
    old_map[res2]=new_map[res2]; //update old map
    new_map[res2]=(int)dist;
    scan_right[res2]=(int)dist;
  }
  else if(i>0 && i<res2+1) { //sweep right
    //move servo to position
    scanner.write(mapscanner(angle[res2-i]));
    delay(dt_servo);
    take_reading();
    //update map
    old_map[res2-i]=new_map[res2-i]; //update old map
    new_map[res2-i]=(int)dist;
    scan_right[res2-i]=(int)dist;
  }
  else if(i>res2 && i<resolution+1) { //sweep left to centre
    //move servo to position
    scanner.write(mapscanner(angle[i-res2-1]));
    delay(dt_servo);
    take_reading();
    //update map|
    new_map[i-res2-1]+=(int)dist;
    new_map[i-res2-1]*=0.5;
    scan_left[i-res2-1]=(int)dist;
  }
  else if(i==resolution+1) { //read front
    scanner.write(mapscanner(angle[res2]));
    delay(dt_servo);
    take_reading();
    //update map by averaging the two values
    new_map[res2]+=(int)dist;
    new_map[res2]*=0.5;
    scan_left[res2]=(int)dist;
  }
  else if(i>resolution+1 && i<resolution*1.5+2) { //sweep left
    //move servo to position
    scanner.write(mapscanner(angle[res2+1+i-resolution-2]));
    delay(dt_servo);
    take_reading();
    //update map
    old_map[res2+1+i-resolution-2]=new_map[res2+1+i-resolution-2]; //update old map
    new_map[res2+1+i-resolution-2]=(int)dist;
    scan_left[res2+1+i-resolution-2]=(int)dist;
  }
  else if(i>resolution*1.5+1 && i<resolution*2+2) { //sweep right to centre
    //move servo to position
    scanner.write(mapscanner(angle[resolution-(i-resolution-res2-2)]));
    delay(dt_servo);
    take_reading();
    //update map|
    new_map[resolution-(i-resolution-res2-2)]+=(int)dist;
    new_map[resolution-(i-resolution-res2-2)]*=0.5;
    scan_right[resolution-(i-resolution-res2-2)]=(int)dist;
  }

}

//map the desired angle to the value required to move the servo to that angle
int mapscanner(int angle) {
  //Serial.println(angle);  //for debugging the code
  float u=angle/180.0;
  //Serial.println(u);  //for debugging the code
  return(scannermin+(u*(scannermax-scannermin)));
}

//take readings from ir sensor
void take_reading() {

    delay(dt_ir); //wait for ir sensor to refresh

    reading=0;                // needs to start at 0 and then n readings will be added to it

    for(j=0;j<n;j++) {
      reading+=analogRead(0); // read from analog input 0
      delay(1);                          // wait one millisecond
    }
    reading/=n;           // finally average the n reading  
    dist=12214.16*pow(reading*0.5,-1.08);  // this calculates the distance 
    if(dist<10) {
      dist=0;
    }
    else if(dist>80) {
      dist=80;
    }
}

There aren't as many comments as usually, when I have a chance I will add some more. The code pretty much waits for 3 inputs from Matlab and then runs the sweep 3 times. Then it waits for another set of 3 parameters from Matlab and so on. It has an LED attached to pin13 to show when it is ready and when it is working. The sweep is different from my first attempts, it starts from the centre and goes right and then left and then right back to the centre.

At the end of the day the I couldn't get the optimiser to work as I desired. The optimiser fmincon doesn't allow integer parameters and the genetic algorithm is very slow and eventually cheated and converged to the minimum time I allowed. This was probably because the infra red sensor voltage changes every 32ms, so if it takes all the reading in this time it minimises the objective function... but doesn't give the optimised timings that I wanted.

I expect to edit this post when I have a chance (I have a PhD viva to prepare for) to clean it up and add more comment...

The test program

This is the program I wrote and uploaded to the Arduino. For my car the steering servo has the range of 48 (full left) to 112 (full right) and 80 is about centre. The electronic speed controller has a range of 0 (full power) to 180 (full brake) and I found 80 makes the car drive at its slowest. Finally my servo to sweep the infra red sensor has a range from 0 to 160 to do a 180 degree sweep with 80 being centre. If you are doing this on your own car then you will need to figure these ranges out.

All Arduino programs need to have setup() and loop() functions (see the Arduino website). Since I am using an electronic speed controller I have treated it as a servo.

This code can be copied and pasted in to the Arduino software and uploaded to the Arduino.


 #include <Servo.h>

Servo sweeper;          // create servo object to control the sweeping servo 
Servo motor;            // create servo object to control the motor 
Servo steering;         // create servo object to control the steering 
float distance[161];    // store the distance from the infra red sensor
boolean driving=false;  // a flag so the car knows if it is moving or not

void setup()
{
  sweeper.attach(6);    // attaches the servo on pin 6 to the servo object 
  motor.attach(3);      // attaches the servo on pin 3 to the servo object 
  steering.attach(9);   // attaches the servo on pin 5 to the servo object 

  Serial.begin(115200); // sets the speed of the serial connection to the computer

  sweeper.write(80);    // sets the sweeper servo to the centre
  motor.write(180);     // 0 full power, 82 power starts to 180 full brake
  steering.write(80);   // sets the steering servo to the centre

  // 10 second countdown to start
  for(int i=10;i>0;i--) {
    Serial.println(i);  // print to the computer when the USB cable is connected
    delay(1000);        // 1 second delay
  }
}

// This runs continuously on the arduino
void loop() {

  // find the space in front of the car
  check_front();

  //decide what to do
  if(distance[80]>79) {  // if there is more than 79cm in front then...
    if(driving==false) {    // if the car thinks it is stopped then ...
      Serial.println("Driving forwards...");
      //drive forwards
      if(steering.read()!=80) { // if the steering servo is not set in the centre then move it to centre
        steering.write(80);        // sets the steering servo to centre
        delay(1000);                // wait one second for this to happen
      }
      motor.write(80); // move very slowly
      driving=true;    // set the driving flag to true
      Serial.println("driving");
    }
  }
  else {
    if(driving==true) {// if the car thinks it is moving then ...
      Serial.println("Stopping...");
      motor.write(180); // switch the brake on
      driving=false;    // set the drving flag to false
      Serial.println("stopped");
    }
 
    //Scan left and right
    scan();
 
    if(distance[0]>(distance[160]-1) && distance[0]>79) { // if there is more space on the right than the left 
      Serial.println("Turning Right...");
      if(steering.read()!=112) {  // if the steering servo is not set to full right then
        steering.write(112);        // sets the steering servo to full right
        delay(1000);                 // wait one second for this to happen
      }
      motor.write(80);           // move very slowly
      delay(1000);                 // wait one second for this to happen
      steering.write(80);       // sets the steering servo to centre
      delay(1000);               // wait one second for this to happen
      driving=true;               // set the driving flag to true
      Serial.println("driving");
    }
    else if(distance[160]>(distance[0]-1) && distance[160]>79) {
      Serial.println("Turning Left...");
      if(steering.read()!=48) {  // if the steering servo is not set to full left then
        steering.write(48);        // sets the steering servo to full left
        delay(1000);                 // wait one second for this to happen
      }
      motor.write(80);            // move very slowly
      delay(1000);                 // wait one second for this to happen
      steering.write(80);         // sets the steering servo to centre
      delay(1000);                 // wait one second for this to happen
      driving=true;                 // set the driving flag to true
      Serial.println("driving");
    }
    else {
      // if it can't go forwards, left or right then wait
      Serial.println("I'm Stuck!...");
    }
  }

}

void scan() { // 180 degree scan in front of the car

  Serial.println("Scanning...");

  int cw[161];      // this array will sort the ADC reading in one direction
  int acw[161];    // this array will sort the ADC reading in the other direction
  int reading;       // used for taking the averages
  float dist;          // used for calculating the distance in cm

  if(sweeper.read()!=0) {  // if the servo is not right then
    sweeper.write(0);        // set the servo to full right
    delay(sweeper.read()*10); // wait until this is done
  }

  //read in first direction  
  for(int pos=0;pos<161;pos++) {
    sweeper.write(pos);  //set the servo position
    delay(10);                 //wait until done
    reading=0;                // needs to start at 0 and then 6 readings will be added to it
    for(int i=0;i<6;i++) {
      reading+=analogRead(0); // read from analog input 0
      delay(1);                          // wait one millisecond
    }
    cw[pos]=reading/6;           // finally average the 6 reading and put in the first array
  }

  //reading in the second direction
  for(int pos=160;pos>-1;pos--) {
    sweeper.write(pos);               //set the servo position
    delay(10);                               //wait until done
    reading=0;                               // needs to start at 0 and then 6 readings will be added to it
    for(int i=0;i<6;i++) {
      reading+=analogRead(0);      // read from analog input 0
      delay(1);                             // wait one millisecond
    }
    acw[pos]=reading/6;         // finally average the 6 reading and put in the second array
  }

  //take the average and convert to distance
  for(int i=0;i<161;i++) {
    dist=12343.85*pow(((cw[i]+acw[i])*0.5),-1.15);  // this formula averages the values of the 2 arrays and 
                                                                                  // calculates the distance 
    if(dist<10) { // the infra red sensor is unreliable below 10 cm so set it to 0cm
      dist=0;
    }
    if(dist>80) { // the infra red sensor is unreliable above 80 cm so set it to 80cm
      dist=80;
    }
    distance[i]=dist;  // put the value dist in to the distance array which contains the actual distances in cm
    Serial.print(i);
    Serial.print(" distance = ");
    Serial.println(distance[i]);   // this is sent to the computer serial monitor to check
  }

  sweeper.write(80); // set the servo back to the centre 
  delay(1000);          // wait one second until it is done

}

void check_front() { //just check in front of the car

  Serial.println("Checking Front...");

  int reading;      // used for taking the averages

  if(sweeper.read()!=80) { // if the steering servo is not set in the centre then move it to centre
    sweeper.write(80);
    delay(abs(sweeper.read()-80)*10);
  }

  //take the readings
  reading=0;  // this code is the same as in the scan() function with no sweep
  for(int i=0;i<6;i++) {
    reading+=analogRead(0);
    delay(1);
  }
  float dist = 12343.85*pow((reading/6),-1.15);
  if(dist<10) {
    dist=0;
  }
  if(dist>80) {
    dist=80;
  }
  distance[80]=dist;  // but the result is put in the centre position in the distance array
  Serial.print("80 distance = ");
  Serial.println(distance[80]);

}