From Imperial College Robotics Society
Jump to: navigation, search

Robot vision. The big dip slightly to the left is a coke bottle..

This page shows how to make a basic radar-style setup with the mbed, which uses a sensor (in this case an IR sensor) to scan backwards and forwards on a servo, and communicate the results to a program on the PC, which will then plot it. It also shows how to use timers on the mbed, which are a more flexible alternative to the wait() commands as they allow you to do multiple things at once, in this case turning the servo and sending the results to the computer at the same time as doing a light show on the built-in LEDs.

The program used to plot the results, in real-time, on the computer is Processing, a programming environment geared towards visual feedback. The code is based on Java and C, and most of the language used here is virtually identical the C/C++ used to code the mbed.


The robot

The code given here uses an SG-5010 servo and the Sharp IR-sensors used in the robotics course. The communication to the computer is done through the USB cable. The pins are;

Servo IR sensor
Yellow = signal from pwm pin Yellow = signal to analog in
Red = +5v Red = +5V
Brown = Ground Black = Ground

The field of view in front of the robot is split up into 50 (in this example) regions (which is the resolution of the radar), which form an array. This array stores the readings from the sensor for each of these directions. This information is tranmitted in the following format; it starts with the index, which will be an integer between 0 and the resolution-1 (i.e. 49), followed by a comma ',' to show there is a break, then the sensor reading for this particular index, then the symbols ',<' to show the end of the data pair. For example, a set of data seen using Putty looks like;


This is read by the computer and then converted into an image of the "terrain" in front of the robot.

(note: To use Putty, you will need to change the baud to 115200, since we need a connection much faster than the default for all the data to be transmitted in realtime.)

mbed Code

  1. #include "mbed.h"
  3. //Load the library used to control the servo
  4. #include "Servo.h"  //http://mbed.org/cookbook/Servo
  6. //Define the outputs for the amazing LED display
  7. DigitalOut led1(LED1);
  8. DigitalOut led2(LED2);
  9. DigitalOut led3(LED3);
  10. DigitalOut led4(LED4);
  12. //Define the input from the IR sensor
  13. AnalogIn IR(p20);
  15. //Set up the serial link to the PC
  16. Serial pc(USBTX, USBRX); // tx, rx
  18. //Set up the pwm out to the servo
  19. Servo myservo(p25); // Between 0.0 and 1.0
  21. //Define two timers
  22. Timer t;//used for the timing of the radar
  23. Timer s;//used for the timing of the light display
  25. //Define the resolution i.e. how many points there are on the radar.
  26. //Needs to be const because it is used to define arrays which need to
  27. //be a constant size. This needs to be the same size on the mbed and
  28. //the PC to work properly.
  29. const int resolution=50;
  31. ////Light show
  32. //This is a function that takes an array L[] of size 4, and lights the
  33. //appropriate LEDs. e.g. inputting L[0,1,0,1] will light up LEDs 2 and 4
  34. void pattern(bool L[]) {
  35. if (L[0]==1) led1=1.0; else led1=0.0;
  36. if (L[1]==1) led2=1.0; else led2=0.0;
  37. if (L[2]==1) led3=1.0; else led3=0.0;
  38. if (L[3]==1) led4=1.0; else led4=0.0;
  39. }
  41. //The number of patterns in the light sequence. This needs to be const
  42. //for the same reasons as the resolution above
  43. const int np = 12;
  45. //the frequency i.e. how many different patterns to go through per sec
  46. float freq = 10;
  48. //The actual light pattern. It is a 2D array with where the first index
  49. //defines which LED to control, and the second index says where it is in
  50. //sequence. e.g. lightpattern[3][7] gives the value for LED 3 at step 7
  51. //through the sequence. The program will iterate through each value of np,
  52. //and get values for the 4 LEDs each time.
  53. bool lightpattern[4][np] = 
  54. {{1,1,1,1,0,0,0,0,0,0,1,1},
  55.  {0,0,1,1,1,0,0,0,0,1,1,1},
  56.  {0,0,0,1,1,1,0,0,1,1,1,0},
  57.  {0,0,0,0,1,1,1,1,1,1,0,0}};
  58. //>>>>>>>>>>>>>>>>
  60. int main() {
  61.     //Define the array of the values of the terrain
  62.     float terrain[resolution];
  64.     //Define the reading of the IR sensor
  65.     float localdist;
  67.     //Set the initial position of the servo
  68.     myservo=0.0;
  70.     //Start both timers
  71.     t.start();
  72.     s.start();
  74.     //time is just Timer t multiplied by the multiple panrate, to allow
  75.     //for different panning speeds.
  76.     float time=0.0;
  77.     float panrate=1.0;
  79.     //iv is the index of the terrain we are currently measuring, and iprev
  80.     //is the value that iv had on the last loop (used to tell if we have moved
  81.     //to the next iv ir not)
  82.     int iv=0;
  83.     int iprev=-1;
  85.     //The pulses per second to write to the computer with. Must be the same for the
  86.     //mbed and PC to work
  87.     pc.baud(115200);
  89.     //Repeat indefinitely
  90.     while (1) {
  92.         //Get the value of the IR sensor
  93.         localdist=IR.read();
  95.         //Get time as Timer t * panrate to allow for different speeds
  96.         time=t.read()*panrate;
  98.         //if time>2.0, both sweeps have been completed, so repeat the loop
  99.         if (time>2.0)
  100.         {
  101.             //reset Timer t to 0
  102.             t.reset();
  104.             //reset other values
  105.             time=0;
  106.             iprev=-1.0;
  108.             //wait 50ms
  109.             wait(0.05);
  110.         }
  112.         //For the first half of the sweep
  113.         if (time<1.0)
  114.         {
  115.             //increment myservo from 0.0 to 1.0
  116.             myservo=time;
  118.             //set the index of the terrain for the current position. the (int) in front
  119.             //turns the float into an int and rounds down in the process, so
  120.             //(int)(time*resolution) will give a value between 0 and resolution-1,
  121.             //as time is going from 0.0 to <1.0.
  122.             iv=(int)(time*resolution);
  124.             //set this index to the current sensor reading
  125.             terrain[iv]=localdist;
  127.             //print the value to the serial, only if we haven't already (iv is only different to 
  128.             //iprev the first time it increments)
  129.             if (iv>iprev)
  130.             {pc.printf("%i,",iv);pc.printf("%.3f,<",terrain[iv]);
  131.             iprev=iv;}
  132.         }
  134.         else 
  136.         //The same as for the first half of the sweep, but in reverse.
  137.         {
  138.             myservo=2-time;
  139.             iv=(int)((2-time)*resolution);
  140.             terrain[iv]=localdist;
  141.             if (iv<iprev)
  142.             {pc.printf("%i,",iv);pc.printf("%.3f,<",terrain[iv]);
  143.             iprev=iv;}
  144.         }
  147.         ////Light show
  148.         //define st as the rounded down value of Timer s times the frequency
  149.         int st=floor(s.read()*freq);
  151.         //If we reach the end of the cycle, reset the timer and the step to 0
  152.         if (st>=np) {s.reset(); st=0;}
  154.         //Set the array we want to set the pattern to based on our current step, st,
  155.         //using the lightpattern[] array, and write this to the LEDs using pattern()
  156.         bool lp[4]={lightpattern[0][st],lightpattern[1][st],lightpattern[2][st],lightpattern[3][st]};
  157.         pattern(lp);           
  158.      } 
  159.  }

Processing Code

  1. //Import serial library
  2. import processing.serial.*; 
  4. //Create a serial port
  5. Serial myPort;
  7. //Define the resolution i.e. how many points there are on the radar.
  8. //Since it is used in defining arrays, which need to be a fixed size,
  9. //this needs to be a "final" (the Java version of "const") and can't
  10. //be changed once the program is running. Obvously this value needs to
  11. //be the same on both the PC and the mbed.
  12. final int resolution = 50;
  14. //The array that will store the data points that the radar sees.
  15. float[] terrain = new float[resolution];  
  17. //Setup() is the first function called once the program is started,
  18. //and is used to define properties. It is called only once.
  19. void setup () {
  21.   //Window size
  22.   size(400, 300);
  24.   //Print a list of serial ports to the console at the bottom of the
  25.   //processing window
  26.   println(Serial.list());  
  28.   //This will choose the first serial port on the list to be the one
  29.   //that is used, which is likely to be the mbed. If it isn't, change
  30.   //the Serial.list()[0] array index to the right one from the list
  31.   //printed in the console. '115200' is the baud, which is the number
  32.   //of pulses per second, and needs to be the same for both the PC and
  33.   //the mbed. If there aren't any serial inputs, this is where the
  34.   //program has an error.
  35.   myPort = new Serial(this, Serial.list()[0], 115200);
  37.   //This will call the serialEvent function below when it sees the '<'
  38.   //character
  39.   myPort.bufferUntil('<');
  41.   //Set inital background colour (0=black)
  42.   background(0);
  43. }
  45. void draw () {
  46.   //All the drawing functions are in the serialEvent function, but this
  47.   //empty function needs to be included anyway or it doesnt work
  48. }
  50. //serialEvent is called whenever the '<' character is read from the serial
  51. void serialEvent (Serial myPort) {
  53.   //get the string that we want (e.g. '4,0.56,<')
  54.   String inString = myPort.readStringUntil('<');
  56.   //if the string we just got isn't empty...
  57.   if (inString != null) {
  59.     //Make a temporary array formed of the terrain index and the value of the
  60.     //terrain at that point. the split() function splits the string up using the ','
  61.     //character, which is then converted to a float array, giving;
  62.     //templine[0] = the position of the terrain, between 0 and resolution-1
  63.     //templine[1] = the reading of the sensor at that point, between 0 and 1
  64.     float templine[] = float(split(inString, ","));
  66.     //if the index is actually between 0 and resolution-1, set the terrain at that
  67.     //index to the corresponding value
  68.     if (int(templine[0])>=0 && int(templine[0])<resolution)
  69.       terrain[int(templine[0])]=templine[1];
  71.     //redraw the background in black (removing any lines that were there previously)
  72.     background(0);
  74.     //set the colour of the lines (r,g,b)
  75.     stroke(127,255,0);
  77.     //line(x1,y1,x2,y2)
  78.     //draw the radar trace as it sweeps back and forth, based on the last data to come
  79.     //in (the templine[] array). It streches from -1.0 to +1.0 radians (approx what the
  80.     //servo does)
  81.     line(200, 300, 200+200*sin(-1.0+(2.0/resolution)*templine[0])*(1.0-templine[1]), 
  82.     300-200*cos(-1.0+(2.0/resolution)*templine[0])*(1.0-templine[1]));
  84.     //draw the updated range line across the whole screen, using all values in the terrain[]
  85.     //array, by drawing a line from one point to the next
  86.     for (int i=1;i<resolution;i++)
  87.     {
  88.       stroke(127, 34, 255);//Different colour
  89.       line(200+200*sin(-1.0+(2.0/resolution)*(i-1))*(1.0-terrain[i-1]), 300-200*cos(-1.0+(2.0/resolution)*(i-1))*(1.0-terrain[i-1]), 
  90.       200+200*sin(-1.0+(2.0/resolution)*(i))*(1.0-terrain[i]), 300-200*cos(-1.0+(2.0/resolution)*i)*(1.0-terrain[i]));
  91.     }
  92.   }
  93. }