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@4:ff05e1fe33ce, 2017-03-11 (annotated)

Committer:
iainsc0574
Date:
Sat Mar 11 13:56:22 2017 +0000
Revision:
4:ff05e1fe33ce
Parent:
3:9902261b1156
Child:
8:1af0c46d21d2
added turning math include a dead band UCL = upper control limit, LCL = lower control limit

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