CE201 Embedded : 2022-23 / Mbed 2 deprecated Car1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@6:e15a4b0c8cbf, 2017-03-11 (annotated)

Committer:
fjwats
Date:
Sat Mar 11 13:45:04 2017 +0000
Revision:
6:e15a4b0c8cbf
Parent:
5:0382ed1bf13d
Child:
7:b8a29109ae84
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iainsc0574 0:e0ff0fa89d47 1 #include "mbed.h"
iainsc0574 0:e0ff0fa89d47 2
iainsc0574 0:e0ff0fa89d47 3 #define MAX 0x0A
iainsc0574 1:1c796b8db63c 4 #define HI 0x01
iainsc0574 1:1c796b8db63c 5 #define LO 0x00
iainsc0574 1:1c796b8db63c 6
iainsc0574 0:e0ff0fa89d47 7 DigitalOut led_1(LED1); //program running.
iainsc0574 0:e0ff0fa89d47 8 DigitalOut led_2(LED2); //sensors operating.
iainsc0574 0:e0ff0fa89d47 9 DigitalOut led_3(LED3); //is moving.
iainsc0574 0:e0ff0fa89d47 10 DigitalOut led_4(LED4); //is complete.
fjwats 5:0382ed1bf13d 11
iainsc0574 0:e0ff0fa89d47 12 DigitalOut Bit1(p25);
iainsc0574 0:e0ff0fa89d47 13 DigitalOut Bit2(p24);
iainsc0574 0:e0ff0fa89d47 14 DigitalOut Bit3(p23);
iainsc0574 1:1c796b8db63c 15 DigitalOut Trig(p6);
iainsc0574 1:1c796b8db63c 16 DigitalIn Echo(p7);
fjwats 5:0382ed1bf13d 17
iainsc0574 2:22d86efbbada 18 PwmOut steering(p21);
iainsc0574 2:22d86efbbada 19 PwmOut velocity(p22);
iainsc0574 1:1c796b8db63c 20
iainsc0574 1:1c796b8db63c 21 Timer US;
iainsc0574 0:e0ff0fa89d47 22
iainsc0574 2:22d86efbbada 23 //global variable dec
iainsc0574 0:e0ff0fa89d47 24 int rawUS_data[5]={0,0,0,0,0}; //raw data{chan1,chan2,chan3,chan4,chan5}
iainsc0574 0:e0ff0fa89d47 25 int US1_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 26 int US2_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 27 int US3_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 28 int US4_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 29 int US5_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 1:1c796b8db63c 30 int turn_rate[2]={0,0};
iainsc0574 2:22d86efbbada 31 float vo = 0;
iainsc0574 0:e0ff0fa89d47 32
iainsc0574 2:22d86efbbada 33 //function constructs
iainsc0574 1:1c796b8db63c 34 void setActiveUS(int chan); //select sensor
iainsc0574 1:1c796b8db63c 35 int getPing(void); //get measurement
iainsc0574 2:22d86efbbada 36 void turn(float s); //turn car -1 left 0 centre 1 right
iainsc0574 2:22d86efbbada 37 void drive(float v); //drive -1 reverse 1 forward
iainsc0574 1:1c796b8db63c 38 void stop(void);
iainsc0574 3:9902261b1156 39 int measurement_mean(int chan);
iainsc0574 1:1c796b8db63c 40
iainsc0574 0:e0ff0fa89d47 41 int main() {
iainsc0574 0:e0ff0fa89d47 42 int iCount = 0;
fjwats 5:0382ed1bf13d 43 int measured = 0;
iainsc0574 2:22d86efbbada 44 int iMean = 0;
iainsc0574 2:22d86efbbada 45 double mean_measured1[2] = {0,0};
iainsc0574 2:22d86efbbada 46 double mean_measured2[2] = {0,0};
iainsc0574 2:22d86efbbada 47 double mean_measured3[2] = {0,0};
iainsc0574 2:22d86efbbada 48 double mean_measured4[2] = {0,0};
iainsc0574 2:22d86efbbada 49 double mean_measured5[2] = {0,0};
iainsc0574 2:22d86efbbada 50
iainsc0574 2:22d86efbbada 51 while(iMean < 3){
iainsc0574 2:22d86efbbada 52 if(iMean == 2){
iainsc0574 2:22d86efbbada 53 iMean = 0;
iainsc0574 2:22d86efbbada 54 }
iainsc0574 2:22d86efbbada 55 while(iCount <= 5){
iainsc0574 2:22d86efbbada 56 setActiveUS(iCount); //set mux address
fjwats 5:0382ed1bf13d 57 rawUS_data[iCount] = getPing(); //get raw measurement
iainsc0574 2:22d86efbbada 58 iCount +=1;
fjwats 6:e15a4b0c8cbf 59 wait(0.4); //will need calibrating
fjwats 5:0382ed1bf13d 60 }
fjwats 5:0382ed1bf13d 61
iainsc0574 2:22d86efbbada 62 US1_mean[0]=rawUS_data[0]; //assign new value
iainsc0574 2:22d86efbbada 63 US2_mean[0]=rawUS_data[1]; //to respective sensor
iainsc0574 2:22d86efbbada 64 US3_mean[0]=rawUS_data[2];
iainsc0574 2:22d86efbbada 65 US4_mean[0]=rawUS_data[3];
iainsc0574 2:22d86efbbada 66 US5_mean[0]=rawUS_data[4];
fjwats 5:0382ed1bf13d 67
iainsc0574 2:22d86efbbada 68 for(int i=MAX;i>0;i--){
iainsc0574 2:22d86efbbada 69 US1_mean[i] = US1_mean[i-1]; //index data along window
iainsc0574 2:22d86efbbada 70 US2_mean[i] = US1_mean[i-1]; //wrt size of window
iainsc0574 2:22d86efbbada 71 US3_mean[i] = US1_mean[i-1];
iainsc0574 2:22d86efbbada 72 US4_mean[i] = US1_mean[i-1];
iainsc0574 2:22d86efbbada 73 US5_mean[i] = US1_mean[i-1];
fjwats 5:0382ed1bf13d 74 }
fjwats 5:0382ed1bf13d 75
iainsc0574 3:9902261b1156 76 mean_measured1[0] = measurement_mean(1); //calc mean 1
iainsc0574 3:9902261b1156 77 mean_measured2[0] = measurement_mean(2); //calc mean 2
iainsc0574 3:9902261b1156 78 mean_measured3[0] = measurement_mean(3); //calc mean 3
iainsc0574 3:9902261b1156 79 mean_measured4[0] = measurement_mean(4); //calc mean 4
iainsc0574 3:9902261b1156 80 mean_measured5[0] = measurement_mean(5); //calc mean 5
iainsc0574 2:22d86efbbada 81
iainsc0574 2:22d86efbbada 82 if(iMean == 1){
iainsc0574 2:22d86efbbada 83 mean_measured1[1] = mean_measured1[0];
iainsc0574 2:22d86efbbada 84 mean_measured2[1] = mean_measured2[0];
iainsc0574 2:22d86efbbada 85 mean_measured3[1] = mean_measured3[0];
iainsc0574 2:22d86efbbada 86 mean_measured4[1] = mean_measured4[0];
iainsc0574 2:22d86efbbada 87 mean_measured5[1] = mean_measured5[0];
fjwats 5:0382ed1bf13d 88 }
fjwats 5:0382ed1bf13d 89
iainsc0574 2:22d86efbbada 90 //if the second value is smaller than the first value
iainsc0574 2:22d86efbbada 91 //steer right
iainsc0574 2:22d86efbbada 92
iainsc0574 2:22d86efbbada 93 //if the second value is bigger than first value
iainsc0574 2:22d86efbbada 94 //steer left
iainsc0574 2:22d86efbbada 95
iainsc0574 2:22d86efbbada 96 // with respect to a deadband (error)
iainsc0574 2:22d86efbbada 97
iainsc0574 2:22d86efbbada 98 //drive at 1 speed
iainsc0574 2:22d86efbbada 99 iMean +=1;
iainsc0574 2:22d86efbbada 100 }
iainsc0574 0:e0ff0fa89d47 101 }
iainsc0574 0:e0ff0fa89d47 102
iainsc0574 0:e0ff0fa89d47 103 void setActiveUS(int chan){
iainsc0574 0:e0ff0fa89d47 104 switch(chan){
iainsc0574 0:e0ff0fa89d47 105 case 0:
iainsc0574 0:e0ff0fa89d47 106 //ultrasonic 1
iainsc0574 1:1c796b8db63c 107 Bit1 = 0;
iainsc0574 1:1c796b8db63c 108 Bit2 = 0;
iainsc0574 1:1c796b8db63c 109 Bit3 = 0;
iainsc0574 1:1c796b8db63c 110 return;
iainsc0574 0:e0ff0fa89d47 111
iainsc0574 0:e0ff0fa89d47 112 case 1:
iainsc0574 0:e0ff0fa89d47 113 //ultrasonic 2
iainsc0574 1:1c796b8db63c 114 Bit1 = 1;
iainsc0574 1:1c796b8db63c 115 Bit2 = 0;
iainsc0574 1:1c796b8db63c 116 Bit3 = 0;
iainsc0574 1:1c796b8db63c 117 return;
iainsc0574 0:e0ff0fa89d47 118
iainsc0574 0:e0ff0fa89d47 119 case 2:
iainsc0574 0:e0ff0fa89d47 120 //ultrasonic 3
iainsc0574 1:1c796b8db63c 121 Bit1 = 0;
iainsc0574 1:1c796b8db63c 122 Bit2 = 1;
iainsc0574 1:1c796b8db63c 123 Bit3 = 0;
iainsc0574 1:1c796b8db63c 124 return;
iainsc0574 0:e0ff0fa89d47 125
iainsc0574 0:e0ff0fa89d47 126 case 3:
iainsc0574 0:e0ff0fa89d47 127 //ultrasonic 4
iainsc0574 1:1c796b8db63c 128 Bit1 = 1;
iainsc0574 1:1c796b8db63c 129 Bit2 = 1;
iainsc0574 1:1c796b8db63c 130 Bit3 = 0;
iainsc0574 1:1c796b8db63c 131 return;
iainsc0574 0:e0ff0fa89d47 132
iainsc0574 0:e0ff0fa89d47 133 case 4:
iainsc0574 0:e0ff0fa89d47 134 //ultrasonic 5
iainsc0574 1:1c796b8db63c 135 Bit1 = 0;
iainsc0574 1:1c796b8db63c 136 Bit2 = 0;
iainsc0574 1:1c796b8db63c 137 Bit3 = 1;
iainsc0574 1:1c796b8db63c 138 return;
iainsc0574 0:e0ff0fa89d47 139 }
iainsc0574 0:e0ff0fa89d47 140 }
iainsc0574 0:e0ff0fa89d47 141
iainsc0574 0:e0ff0fa89d47 142 int getPing(void){
fjwats 5:0382ed1bf13d 143 int result=0;
fjwats 5:0382ed1bf13d 144
fjwats 5:0382ed1bf13d 145 Trig = HI; //start positive edge of trigger
iainsc0574 1:1c796b8db63c 146 US.reset();
iainsc0574 2:22d86efbbada 147 wait_us(10.0); //hold it high for 10us
iainsc0574 2:22d86efbbada 148 Trig = LO; //set negative going edge
iainsc0574 1:1c796b8db63c 149 while(Echo == 0){}; //while receive is LO
iainsc0574 1:1c796b8db63c 150 US.start(); //start counting received pulse
iainsc0574 1:1c796b8db63c 151 while(Echo == 1){};
iainsc0574 1:1c796b8db63c 152 US.stop();
iainsc0574 1:1c796b8db63c 153 result = ((US.read_us()*10)/58); //pulse duration = distance set to cm
iainsc0574 1:1c796b8db63c 154 return result;
iainsc0574 1:1c796b8db63c 155 }
iainsc0574 1:1c796b8db63c 156
fjwats 5:0382ed1bf13d 157 void turn(float t){
fjwats 5:0382ed1bf13d 158 if (t++>=0&&s<=2) {steering.pulsewidth(t/2000+0.001);} // calculate steering angle to pulsewidth conversion
fjwats 5:0382ed1bf13d 159 telemUpdate(); // save telem file after angle change
fjwats 5:0382ed1bf13d 160 return;
fjwats 5:0382ed1bf13d 161 }
iainsc0574 1:1c796b8db63c 162
iainsc0574 2:22d86efbbada 163 void drive(float v){
fjwats 5:0382ed1bf13d 164 if (v++>=0&&s<=2) {velocity.pulsewidth(v/2000+0.001);} // calculate velocity to pulsewidth conversion
fjwats 5:0382ed1bf13d 165 telemUpdate(0.0); // save telem file after speed change
iainsc0574 1:1c796b8db63c 166 return;
fjwats 5:0382ed1bf13d 167 }
fjwats 5:0382ed1bf13d 168
iainsc0574 1:1c796b8db63c 169 void stop(void){
fjwats 5:0382ed1bf13d 170 drive(0.0); // Reduce speed to 0
iainsc0574 1:1c796b8db63c 171 return;
fjwats 5:0382ed1bf13d 172 }
iainsc0574 1:1c796b8db63c 173
iainsc0574 3:9902261b1156 174 int measurement_mean(int chan){
iainsc0574 1:1c796b8db63c 175 int value=0;
iainsc0574 1:1c796b8db63c 176 int sum = 0;
iainsc0574 3:9902261b1156 177 switch (chan){
iainsc0574 3:9902261b1156 178 case 1:
iainsc0574 3:9902261b1156 179 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 180 sum += US1_mean[i]; //sum all elements of US1 window
iainsc0574 3:9902261b1156 181 }
iainsc0574 3:9902261b1156 182 value = sum/MAX; //calculate mean for US1
iainsc0574 3:9902261b1156 183 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 184 case 2:
iainsc0574 3:9902261b1156 185 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 186 sum +=US2_mean[i]; //sum all elements of US2 window
iainsc0574 3:9902261b1156 187 }
iainsc0574 3:9902261b1156 188 value = sum/MAX; //calculate mean for US2
iainsc0574 3:9902261b1156 189 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 190 case 3:
iainsc0574 3:9902261b1156 191 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 192 sum +=US3_mean[i]; //sum all elements of US3 window
iainsc0574 3:9902261b1156 193 }
iainsc0574 3:9902261b1156 194 value = sum/MAX; //calculate mean for US3
iainsc0574 3:9902261b1156 195 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 196 case 4:
iainsc0574 3:9902261b1156 197 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 198 sum +=US4_mean[i]; //sum all elements of US4 window
iainsc0574 3:9902261b1156 199 }
iainsc0574 3:9902261b1156 200 value = sum/MAX; //calculate mean for US4
iainsc0574 3:9902261b1156 201 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 202 case 5:
iainsc0574 3:9902261b1156 203 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 204 sum +=US5_mean[i]; //sum all elements of US5 window
iainsc0574 3:9902261b1156 205 }
iainsc0574 3:9902261b1156 206 value = sum/MAX; //calculate mean for US5
iainsc0574 3:9902261b1156 207 return (int)value; //return using type casting
iainsc0574 1:1c796b8db63c 208 }
iainsc0574 3:9902261b1156 209
iainsc0574 3:9902261b1156 210 return 0;
iainsc0574 1:1c796b8db63c 211 }