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.
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...
No comments:
Post a Comment