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

Committer:
fjwats
Date:
Sat Mar 11 14:09:00 2017 +0000
Revision:
7:b8a29109ae84
Parent:
6:e15a4b0c8cbf
Child:
8:1af0c46d21d2
temp pin set, gettemp included but not finished, telemupdate(s) finished, stop/drive/steer optimised, probably a few more small things

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