University of Derby - MSc Control and Instrumentation / Mbed 2 deprecated Moon_Buggy_Obstacle

The Program for our autonomous vehicle which we built for our course project

Dependencies:   mbed

Fork of Moon_Buggy_Obstacle by Jamie Gibson

main.cpp@0:49968ee36869, 2015-12-10 (annotated)

Committer:
JamieGibson
Date:
Thu Dec 10 14:47:38 2015 +0000
Revision:
0:49968ee36869
Moon Buggy Obstacle Sensing Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamieGibson 0:49968ee36869 1 #include "mbed.h"
JamieGibson 0:49968ee36869 2 SPI ser_port(p11, p12, p13); // mosi, miso, sclk, ssel
JamieGibson 0:49968ee36869 3 DigitalOut cs(p14); //this acts as “slave select”
JamieGibson 0:49968ee36869 4 Serial pc(USBTX, USBRX);
JamieGibson 0:49968ee36869 5 DigitalIn echo(p20);
JamieGibson 0:49968ee36869 6 DigitalOut trigger(p19);
JamieGibson 0:49968ee36869 7 DigitalIn echo1(p18);
JamieGibson 0:49968ee36869 8 DigitalOut trigger1(p17);
JamieGibson 0:49968ee36869 9 DigitalIn echo2(p16);
JamieGibson 0:49968ee36869 10 DigitalOut trigger2(p15);
JamieGibson 0:49968ee36869 11 DigitalOut greenled(p21);
JamieGibson 0:49968ee36869 12 DigitalOut redled(p22);
JamieGibson 0:49968ee36869 13 DigitalOut yellowled(p23);
JamieGibson 0:49968ee36869 14 char switch_word ; //word we will send
JamieGibson 0:49968ee36869 15
JamieGibson 0:49968ee36869 16 Timer t;
JamieGibson 0:49968ee36869 17 float i,j,k;
JamieGibson 0:49968ee36869 18
JamieGibson 0:49968ee36869 19 int main()
JamieGibson 0:49968ee36869 20 {
JamieGibson 0:49968ee36869 21 ser_port.format(8,0);
JamieGibson 0:49968ee36869 22 ser_port.frequency(4000000);
JamieGibson 0:49968ee36869 23 t.start(); //start timer
JamieGibson 0:49968ee36869 24 while(1)
JamieGibson 0:49968ee36869 25 {
JamieGibson 0:49968ee36869 26 trigger = 1;
JamieGibson 0:49968ee36869 27 wait_ms(1);
JamieGibson 0:49968ee36869 28 trigger=0; //stop sending pulses
JamieGibson 0:49968ee36869 29 while(!echo); //listen for echo pulse
JamieGibson 0:49968ee36869 30 t.reset(); //reset timer to measure echo pulse width
JamieGibson 0:49968ee36869 31 while(echo);
JamieGibson 0:49968ee36869 32 i=t.read_us(); //attach i to echo pulse width of sensor 1 in us
JamieGibson 0:49968ee36869 33
JamieGibson 0:49968ee36869 34 trigger1 = 1;
JamieGibson 0:49968ee36869 35 wait_ms(1);
JamieGibson 0:49968ee36869 36 trigger1=0; //stop sending pulses
JamieGibson 0:49968ee36869 37 while(!echo1); //listen for echo pulse
JamieGibson 0:49968ee36869 38 t.reset(); //reset timer to measure echo pulse width
JamieGibson 0:49968ee36869 39 while(echo1);
JamieGibson 0:49968ee36869 40 j=t.read_us(); //attach i to echo pulse width of sensor 1 in us
JamieGibson 0:49968ee36869 41
JamieGibson 0:49968ee36869 42 trigger2 = 1;
JamieGibson 0:49968ee36869 43 wait_ms(1);
JamieGibson 0:49968ee36869 44 trigger2=0; //stop sending pulses
JamieGibson 0:49968ee36869 45 while(!echo2); //listen for echo pulse
JamieGibson 0:49968ee36869 46 t.reset(); //reset timer to measure echo pulse width
JamieGibson 0:49968ee36869 47 while(echo2);
JamieGibson 0:49968ee36869 48 k=t.read_us(); //attach i to echo pulse width of sensor 1 in us
JamieGibson 0:49968ee36869 49
JamieGibson 0:49968ee36869 50 i=i/58; //converting to cm
JamieGibson 0:49968ee36869 51 j=j/58;
JamieGibson 0:49968ee36869 52 k=k/58;
JamieGibson 0:49968ee36869 53 pc.printf("Distance of i\n\r");
JamieGibson 0:49968ee36869 54 pc.printf("%1.3f \n\r",i);
JamieGibson 0:49968ee36869 55 wait(0.10);
JamieGibson 0:49968ee36869 56 pc.printf("Distance of j\n\r");
JamieGibson 0:49968ee36869 57 pc.printf("%1.3f \n\r",j);
JamieGibson 0:49968ee36869 58 wait(0.10);
JamieGibson 0:49968ee36869 59 pc.printf("Distance of k\n\r");
JamieGibson 0:49968ee36869 60 pc.printf("%1.3f \n\r",k);
JamieGibson 0:49968ee36869 61 wait(0.10);
JamieGibson 0:49968ee36869 62
JamieGibson 0:49968ee36869 63 greenled = 0;
JamieGibson 0:49968ee36869 64 redled = 0;
JamieGibson 0:49968ee36869 65 yellowled = 0;
JamieGibson 0:49968ee36869 66
JamieGibson 0:49968ee36869 67 switch_word=0xF0; //set up a recognisable output pattern
JamieGibson 0:49968ee36869 68 if((i>75) &&(j>75)&&(k>75)){ // condition for no obstacle
JamieGibson 0:49968ee36869 69 switch_word=switch_word|0x00; //move forward
JamieGibson 0:49968ee36869 70 }
JamieGibson 0:49968ee36869 71 if((i<75)&&(j>75)&&(k>75)){ //sensor 1 senses obstacle
JamieGibson 0:49968ee36869 72 switch_word=switch_word|0x01; // obstacle in the centre; turn left
JamieGibson 0:49968ee36869 73 yellowled=1;
JamieGibson 0:49968ee36869 74 }
JamieGibson 0:49968ee36869 75 if ((j<75)&&(i>75)&&(k>75)){
JamieGibson 0:49968ee36869 76 switch_word=switch_word|0x02; // obstacle on the right; turn left
JamieGibson 0:49968ee36869 77 greenled=1;
JamieGibson 0:49968ee36869 78 }
JamieGibson 0:49968ee36869 79 if((k<75)&&(j>75)&&(i>75)){
JamieGibson 0:49968ee36869 80 switch_word=switch_word|0x03; // obstacle on the left; turn right
JamieGibson 0:49968ee36869 81 redled=1;
JamieGibson 0:49968ee36869 82 }
JamieGibson 0:49968ee36869 83 if((i<75)&&(j<75)&&(k<75)){
JamieGibson 0:49968ee36869 84 switch_word= switch_word|0x04; // no way clear; reverse
JamieGibson 0:49968ee36869 85 greenled=1;
JamieGibson 0:49968ee36869 86 redled=1;
JamieGibson 0:49968ee36869 87 yellowled=1;
JamieGibson 0:49968ee36869 88 }
JamieGibson 0:49968ee36869 89 if((i<75)&&(j<75)&&(k>75)){
JamieGibson 0:49968ee36869 90 switch_word= switch_word|0x05; // FR
JamieGibson 0:49968ee36869 91 greenled=1;
JamieGibson 0:49968ee36869 92 yellowled=1;
JamieGibson 0:49968ee36869 93 }
JamieGibson 0:49968ee36869 94 if((i<75)&&(k<75)&&(j>75)){
JamieGibson 0:49968ee36869 95 switch_word= switch_word|0x06; // FL
JamieGibson 0:49968ee36869 96 redled=1;
JamieGibson 0:49968ee36869 97 yellowled=1;
JamieGibson 0:49968ee36869 98 }
JamieGibson 0:49968ee36869 99 cs = 0; //select slave
JamieGibson 0:49968ee36869 100 ser_port.write(switch_word); //send switch_word
JamieGibson 0:49968ee36869 101 cs = 1;
JamieGibson 0:49968ee36869 102 wait (0.000002);
JamieGibson 0:49968ee36869 103 }
JamieGibson 0:49968ee36869 104 }