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

Committer:
fjwats
Date:
Sat Mar 11 15:18:15 2017 +0000
Revision:
9:4780720613bb
Parent:
8:1af0c46d21d2 
changed how telemUpdate works, all data is now written at once from one routine that takes no args. Uses global vars "curWheelAngle" and "curVelocity". getTemp routine now returns temperature in float so no temp is actually stored in the program.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iainsc0574 0:e0ff0fa89d47 1 #include "mbed.h"
fjwats 9:4780720613bb 2 #include <fstream>
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
fjwats 9:4780720613bb 8 //pin declarations
iainsc0574 0:e0ff0fa89d47 9 DigitalOut led_1(LED1); //program running.
iainsc0574 0:e0ff0fa89d47 10 DigitalOut led_2(LED2); //sensors operating.
iainsc0574 0:e0ff0fa89d47 11 DigitalOut led_3(LED3); //is moving.
iainsc0574 0:e0ff0fa89d47 12 DigitalOut led_4(LED4); //is complete.
fjwats 5:0382ed1bf13d 13
fjwats 7:b8a29109ae84 14 DigitalOut Bit1(p25); // US mux value
fjwats 7:b8a29109ae84 15 DigitalOut Bit2(p24); // US mux value
fjwats 7:b8a29109ae84 16 DigitalOut Bit3(p23); // US mux value
fjwats 7:b8a29109ae84 17 DigitalOut Trig(p6); // outgoing pin
fjwats 7:b8a29109ae84 18 DigitalIn Echo(p7); // recieving pin
iainsc0574 1:1c796b8db63c 19
fjwats 7:b8a29109ae84 20 AnalogIn temperature(p20); // temperature read pin
iainsc0574 0:e0ff0fa89d47 21
fjwats 7:b8a29109ae84 22 PwmOut steering(p21); // steering modification
fjwats 7:b8a29109ae84 23 PwmOut velocity(p22); // velocity modification
fjwats 7:b8a29109ae84 24
fjwats 9:4780720613bb 25 Timer US; // measures time between ping and pong in getPing() routine
iainsc0574 2:22d86efbbada 26
iainsc0574 2:22d86efbbada 27 //global variable dec
iainsc0574 0:e0ff0fa89d47 28 int rawUS_data[5]={0,0,0,0,0}; //raw data{chan1,chan2,chan3,chan4,chan5}
iainsc0574 0:e0ff0fa89d47 29 int US1_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 30 int US2_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 31 int US3_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 32 int US4_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 0:e0ff0fa89d47 33 int US5_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
iainsc0574 1:1c796b8db63c 34 int turn_rate[2]={0,0};
fjwats 9:4780720613bb 35
fjwats 9:4780720613bb 36 float curVelocity = 0.0; // current speed the car is travelling
fjwats 9:4780720613bb 37 float curWheelAngle = 0.0; // current angle the wheels are set to
fjwats 9:4780720613bb 38
fjwats 9:4780720613bb 39 std::ofstream stream ("DriveData.txt"); // file object for holding info about telem data gathered since last power on
iainsc0574 0:e0ff0fa89d47 40
iainsc0574 2:22d86efbbada 41 //function constructs
iainsc0574 1:1c796b8db63c 42 void setActiveUS(int chan); //select sensor
fjwats 7:b8a29109ae84 43 int getPing(void); //get US measurement
fjwats 7:b8a29109ae84 44 void turn(float s); //turn car -1 left 0 centre 1 right
fjwats 7:b8a29109ae84 45 void drive(float v); //drive -1 reverse 1 forward
fjwats 7:b8a29109ae84 46 void stop(void); //stop the car
fjwats 9:4780720613bb 47 float getTemp(void); //read a temp from the sensor
iainsc0574 3:9902261b1156 48 int measurement_mean(int chan);
iainsc0574 1:1c796b8db63c 49
iainsc0574 0:e0ff0fa89d47 50 int main() {
iainsc0574 0:e0ff0fa89d47 51 int iCount = 0;
fjwats 5:0382ed1bf13d 52 int measured = 0;
iainsc0574 2:22d86efbbada 53 int iMean = 0;
fjwats 9:4780720613bb 54 double mean_measured1[2] = {0,0}; // <-- Iain could you comment the local variables pls
iainsc0574 2:22d86efbbada 55 double mean_measured2[2] = {0,0};
iainsc0574 2:22d86efbbada 56 double mean_measured3[2] = {0,0};
iainsc0574 2:22d86efbbada 57 double mean_measured4[2] = {0,0};
iainsc0574 2:22d86efbbada 58 double mean_measured5[2] = {0,0};
iainsc0574 2:22d86efbbada 59
iainsc0574 2:22d86efbbada 60 while(iMean < 3){
iainsc0574 2:22d86efbbada 61 if(iMean == 2){
iainsc0574 2:22d86efbbada 62 iMean = 0;
iainsc0574 2:22d86efbbada 63 }
iainsc0574 2:22d86efbbada 64 while(iCount <= 5){
iainsc0574 2:22d86efbbada 65 setActiveUS(iCount); //set mux address
fjwats 5:0382ed1bf13d 66 rawUS_data[iCount] = getPing(); //get raw measurement
iainsc0574 2:22d86efbbada 67 iCount +=1;
fjwats 9:4780720613bb 68 wait(0.4); // <-- will need calibrating
fjwats 5:0382ed1bf13d 69 }
fjwats 5:0382ed1bf13d 70
iainsc0574 2:22d86efbbada 71 US1_mean[0]=rawUS_data[0]; //assign new value
iainsc0574 2:22d86efbbada 72 US2_mean[0]=rawUS_data[1]; //to respective sensor
iainsc0574 2:22d86efbbada 73 US3_mean[0]=rawUS_data[2];
iainsc0574 2:22d86efbbada 74 US4_mean[0]=rawUS_data[3];
iainsc0574 2:22d86efbbada 75 US5_mean[0]=rawUS_data[4];
fjwats 5:0382ed1bf13d 76
iainsc0574 2:22d86efbbada 77 for(int i=MAX;i>0;i--){
iainsc0574 2:22d86efbbada 78 US1_mean[i] = US1_mean[i-1]; //index data along window
iainsc0574 2:22d86efbbada 79 US2_mean[i] = US1_mean[i-1]; //wrt size of window
iainsc0574 2:22d86efbbada 80 US3_mean[i] = US1_mean[i-1];
iainsc0574 2:22d86efbbada 81 US4_mean[i] = US1_mean[i-1];
iainsc0574 2:22d86efbbada 82 US5_mean[i] = US1_mean[i-1];
fjwats 5:0382ed1bf13d 83 }
fjwats 5:0382ed1bf13d 84
iainsc0574 3:9902261b1156 85 mean_measured1[0] = measurement_mean(1); //calc mean 1
iainsc0574 3:9902261b1156 86 mean_measured2[0] = measurement_mean(2); //calc mean 2
iainsc0574 3:9902261b1156 87 mean_measured3[0] = measurement_mean(3); //calc mean 3
iainsc0574 3:9902261b1156 88 mean_measured4[0] = measurement_mean(4); //calc mean 4
iainsc0574 3:9902261b1156 89 mean_measured5[0] = measurement_mean(5); //calc mean 5
iainsc0574 2:22d86efbbada 90
iainsc0574 2:22d86efbbada 91 if(iMean == 1){
iainsc0574 2:22d86efbbada 92 mean_measured1[1] = mean_measured1[0];
iainsc0574 2:22d86efbbada 93 mean_measured2[1] = mean_measured2[0];
iainsc0574 2:22d86efbbada 94 mean_measured3[1] = mean_measured3[0];
iainsc0574 2:22d86efbbada 95 mean_measured4[1] = mean_measured4[0];
iainsc0574 2:22d86efbbada 96 mean_measured5[1] = mean_measured5[0];
fjwats 5:0382ed1bf13d 97 }
fjwats 5:0382ed1bf13d 98
iainsc0574 2:22d86efbbada 99 //if the second value is smaller than the first value
iainsc0574 2:22d86efbbada 100 //steer right
iainsc0574 4:ff05e1fe33ce 101 int value_to_steer_UCL = 1;
iainsc0574 4:ff05e1fe33ce 102 int value_to_steer_LCL = -1;
fjwats 9:4780720613bb 103 int val1=0; // left side
fjwats 9:4780720613bb 104 int val2=0; // left quarter
fjwats 9:4780720613bb 105 int val3=0; // forward
fjwats 9:4780720613bb 106 int val4=0; // right quarter
fjwats 9:4780720613bb 107 int val5=0; // right side
fjwats 9:4780720613bb 108
iainsc0574 4:ff05e1fe33ce 109 val1 = mean_measured1[1] - mean_measured1[0];
iainsc0574 4:ff05e1fe33ce 110 val2 = mean_measured2[1] - mean_measured2[0];
iainsc0574 4:ff05e1fe33ce 111 val3 = mean_measured3[1] - mean_measured3[0];
iainsc0574 4:ff05e1fe33ce 112 val4 = mean_measured4[1] - mean_measured4[0];
iainsc0574 4:ff05e1fe33ce 113 val5 = mean_measured5[1] - mean_measured5[0];
iainsc0574 2:22d86efbbada 114
iainsc0574 4:ff05e1fe33ce 115 if((val2 < value_to_steer_LCL) && (val4 > value_to_steer_UCL)){
iainsc0574 4:ff05e1fe33ce 116 //turn right
fjwats 9:4780720613bb 117 //curWheelAngle += 10;
fjwats 9:4780720613bb 118 //drive(curWheelAngle);
iainsc0574 4:ff05e1fe33ce 119 }
iainsc0574 4:ff05e1fe33ce 120 else if((val2 > value_to_steer_UCL) && (val4 < value_to_steer_LCL)){
iainsc0574 4:ff05e1fe33ce 121 //turn left
fjwats 9:4780720613bb 122 //curWheelAngle += 10;
fjwats 9:4780720613bb 123 //drive(curWheelAngle);
iainsc0574 4:ff05e1fe33ce 124 }
iainsc0574 4:ff05e1fe33ce 125 else {
iainsc0574 4:ff05e1fe33ce 126 // drive
fjwats 9:4780720613bb 127 // <-- do we need an else here? the car will continue travelling anyway
iainsc0574 4:ff05e1fe33ce 128 }
iainsc0574 2:22d86efbbada 129
iainsc0574 2:22d86efbbada 130 //drive at 1 speed
iainsc0574 2:22d86efbbada 131 iMean +=1;
fjwats 9:4780720613bb 132 }
iainsc0574 0:e0ff0fa89d47 133 }
iainsc0574 0:e0ff0fa89d47 134
iainsc0574 0:e0ff0fa89d47 135 void setActiveUS(int chan){
iainsc0574 0:e0ff0fa89d47 136 switch(chan){
iainsc0574 0:e0ff0fa89d47 137 case 0:
iainsc0574 0:e0ff0fa89d47 138 //ultrasonic 1
iainsc0574 1:1c796b8db63c 139 Bit1 = 0;
iainsc0574 1:1c796b8db63c 140 Bit2 = 0;
iainsc0574 1:1c796b8db63c 141 Bit3 = 0;
iainsc0574 1:1c796b8db63c 142 return;
iainsc0574 0:e0ff0fa89d47 143
iainsc0574 0:e0ff0fa89d47 144 case 1:
iainsc0574 0:e0ff0fa89d47 145 //ultrasonic 2
iainsc0574 1:1c796b8db63c 146 Bit1 = 1;
iainsc0574 1:1c796b8db63c 147 Bit2 = 0;
iainsc0574 1:1c796b8db63c 148 Bit3 = 0;
iainsc0574 1:1c796b8db63c 149 return;
iainsc0574 0:e0ff0fa89d47 150
iainsc0574 0:e0ff0fa89d47 151 case 2:
iainsc0574 0:e0ff0fa89d47 152 //ultrasonic 3
iainsc0574 1:1c796b8db63c 153 Bit1 = 0;
iainsc0574 1:1c796b8db63c 154 Bit2 = 1;
iainsc0574 1:1c796b8db63c 155 Bit3 = 0;
iainsc0574 1:1c796b8db63c 156 return;
iainsc0574 0:e0ff0fa89d47 157
iainsc0574 0:e0ff0fa89d47 158 case 3:
iainsc0574 0:e0ff0fa89d47 159 //ultrasonic 4
iainsc0574 1:1c796b8db63c 160 Bit1 = 1;
iainsc0574 1:1c796b8db63c 161 Bit2 = 1;
iainsc0574 1:1c796b8db63c 162 Bit3 = 0;
iainsc0574 1:1c796b8db63c 163 return;
iainsc0574 0:e0ff0fa89d47 164
iainsc0574 0:e0ff0fa89d47 165 case 4:
iainsc0574 0:e0ff0fa89d47 166 //ultrasonic 5
iainsc0574 1:1c796b8db63c 167 Bit1 = 0;
iainsc0574 1:1c796b8db63c 168 Bit2 = 0;
iainsc0574 1:1c796b8db63c 169 Bit3 = 1;
iainsc0574 1:1c796b8db63c 170 return;
iainsc0574 0:e0ff0fa89d47 171 }
iainsc0574 0:e0ff0fa89d47 172 }
iainsc0574 0:e0ff0fa89d47 173
iainsc0574 0:e0ff0fa89d47 174 int getPing(void){
fjwats 5:0382ed1bf13d 175 int result=0;
fjwats 5:0382ed1bf13d 176
fjwats 5:0382ed1bf13d 177 Trig = HI; //start positive edge of trigger
iainsc0574 1:1c796b8db63c 178 US.reset();
iainsc0574 2:22d86efbbada 179 wait_us(10.0); //hold it high for 10us
iainsc0574 2:22d86efbbada 180 Trig = LO; //set negative going edge
iainsc0574 1:1c796b8db63c 181 while(Echo == 0){}; //while receive is LO
iainsc0574 1:1c796b8db63c 182 US.start(); //start counting received pulse
iainsc0574 1:1c796b8db63c 183 while(Echo == 1){};
iainsc0574 1:1c796b8db63c 184 US.stop();
iainsc0574 1:1c796b8db63c 185 result = ((US.read_us()*10)/58); //pulse duration = distance set to cm
iainsc0574 1:1c796b8db63c 186 return result;
iainsc0574 1:1c796b8db63c 187 }
iainsc0574 1:1c796b8db63c 188
fjwats 5:0382ed1bf13d 189 void turn(float t){
fjwats 9:4780720613bb 190 if (t++>=0 && t<=2) {steering.pulsewidth(t/2000+0.001);} // convert steering angle into a pulsewidth
fjwats 9:4780720613bb 191 curWheelAngle = t;
fjwats 9:4780720613bb 192 return;
fjwats 5:0382ed1bf13d 193 }
iainsc0574 1:1c796b8db63c 194
iainsc0574 2:22d86efbbada 195 void drive(float v){
fjwats 9:4780720613bb 196 if (v++>=0 && v<=2) {velocity.pulsewidth(v/2000+0.001);} // convert velocity into a pulsewidth
fjwats 9:4780720613bb 197 curVelocity = v; // set current velocity
iainsc0574 1:1c796b8db63c 198 return;
fjwats 5:0382ed1bf13d 199 }
fjwats 5:0382ed1bf13d 200
iainsc0574 1:1c796b8db63c 201 void stop(void){
fjwats 9:4780720613bb 202 drive(0.0); // Reduce speed to 0, regardless of direction car is travelling
fjwats 9:4780720613bb 203 curVelocity = 0.0;
iainsc0574 1:1c796b8db63c 204 return;
fjwats 5:0382ed1bf13d 205 }
iainsc0574 1:1c796b8db63c 206
fjwats 9:4780720613bb 207 float getTemp(void) { // Reads ambient temperature from Variable Resistor
fjwats 9:4780720613bb 208 return temperature; // <-- Unfinished, needs algorithm for voltage to temp conversion
fjwats 7:b8a29109ae84 209 }
fjwats 7:b8a29109ae84 210
fjwats 9:4780720613bb 211 void telemUpdate() { // Opens set file for logging, appends some data, then returns.
fjwats 9:4780720613bb 212 stream << "Steering angle:" << curWheelAngle << " Velocity:" << curVelocity;// Writes values for speed and heading changes.
fjwats 9:4780720613bb 213 stream << " Ambient Temp:" << getTemp() << "\n"; // Writes value for temp sensor
fjwats 9:4780720613bb 214 return; // <-- If possible we should timestamp each telemUpdate in the file
fjwats 7:b8a29109ae84 215 }
iainsc0574 1:1c796b8db63c 216
iainsc0574 3:9902261b1156 217 int measurement_mean(int chan){
iainsc0574 1:1c796b8db63c 218 int value=0;
iainsc0574 1:1c796b8db63c 219 int sum = 0;
iainsc0574 3:9902261b1156 220 switch (chan){
iainsc0574 3:9902261b1156 221 case 1:
iainsc0574 3:9902261b1156 222 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 223 sum += US1_mean[i]; //sum all elements of US1 window
iainsc0574 3:9902261b1156 224 }
iainsc0574 3:9902261b1156 225 value = sum/MAX; //calculate mean for US1
iainsc0574 3:9902261b1156 226 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 227 case 2:
iainsc0574 3:9902261b1156 228 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 229 sum +=US2_mean[i]; //sum all elements of US2 window
iainsc0574 3:9902261b1156 230 }
iainsc0574 3:9902261b1156 231 value = sum/MAX; //calculate mean for US2
iainsc0574 3:9902261b1156 232 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 233 case 3:
iainsc0574 3:9902261b1156 234 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 235 sum +=US3_mean[i]; //sum all elements of US3 window
iainsc0574 3:9902261b1156 236 }
iainsc0574 3:9902261b1156 237 value = sum/MAX; //calculate mean for US3
iainsc0574 3:9902261b1156 238 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 239 case 4:
iainsc0574 3:9902261b1156 240 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 241 sum +=US4_mean[i]; //sum all elements of US4 window
iainsc0574 3:9902261b1156 242 }
iainsc0574 3:9902261b1156 243 value = sum/MAX; //calculate mean for US4
iainsc0574 3:9902261b1156 244 return (int)value; //return using type casting
iainsc0574 3:9902261b1156 245 case 5:
iainsc0574 3:9902261b1156 246 for(int i=0;i<=MAX;i++){
iainsc0574 3:9902261b1156 247 sum +=US5_mean[i]; //sum all elements of US5 window
iainsc0574 3:9902261b1156 248 }
iainsc0574 3:9902261b1156 249 value = sum/MAX; //calculate mean for US5
iainsc0574 3:9902261b1156 250 return (int)value; //return using type casting
iainsc0574 1:1c796b8db63c 251 }
iainsc0574 3:9902261b1156 252
iainsc0574 3:9902261b1156 253 return 0;
iainsc0574 1:1c796b8db63c 254 }