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

Committer:
iainsc0574
Date:
Sat Mar 11 12:44:14 2017 +0000
Revision:
2:22d86efbbada
Parent:
1:1c796b8db63c
Child:
3:9902261b1156
drive & steering added including mean calcs and rate of change wrt means

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 2:22d86efbbada 40 int measurement_mean(int *Array);
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 2:22d86efbbada 77 mean_measured1[0] = measurement_mean(&US1_mean);
iainsc0574 2:22d86efbbada 78 mean_measured2[0] = measurement_mean(&US2_mean);
iainsc0574 2:22d86efbbada 79 mean_measured3[0] = measurement_mean(&US3_mean);
iainsc0574 2:22d86efbbada 80 mean_measured4[0] = measurement_mean(&US4_mean);
iainsc0574 2:22d86efbbada 81 mean_measured5[0] = measurement_mean(&US5_mean);
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 2:22d86efbbada 93
iainsc0574 2:22d86efbbada 94 //if the second value is bigger than first value
iainsc0574 2:22d86efbbada 95 //steer left
iainsc0574 2:22d86efbbada 96
iainsc0574 2:22d86efbbada 97 // with respect to a deadband (error)
iainsc0574 2:22d86efbbada 98
iainsc0574 2:22d86efbbada 99 //drive at 1 speed
iainsc0574 2:22d86efbbada 100 iMean +=1;
iainsc0574 2:22d86efbbada 101 }
iainsc0574 0:e0ff0fa89d47 102 }
iainsc0574 0:e0ff0fa89d47 103
iainsc0574 0:e0ff0fa89d47 104 void setActiveUS(int chan){
iainsc0574 0:e0ff0fa89d47 105 switch(chan){
iainsc0574 0:e0ff0fa89d47 106 case 0:
iainsc0574 0:e0ff0fa89d47 107 //ultrasonic 1
iainsc0574 1:1c796b8db63c 108 Bit1 = 0;
iainsc0574 1:1c796b8db63c 109 Bit2 = 0;
iainsc0574 1:1c796b8db63c 110 Bit3 = 0;
iainsc0574 1:1c796b8db63c 111 return;
iainsc0574 0:e0ff0fa89d47 112
iainsc0574 0:e0ff0fa89d47 113 case 1:
iainsc0574 0:e0ff0fa89d47 114 //ultrasonic 2
iainsc0574 1:1c796b8db63c 115 Bit1 = 1;
iainsc0574 1:1c796b8db63c 116 Bit2 = 0;
iainsc0574 1:1c796b8db63c 117 Bit3 = 0;
iainsc0574 1:1c796b8db63c 118 return;
iainsc0574 0:e0ff0fa89d47 119
iainsc0574 0:e0ff0fa89d47 120 case 2:
iainsc0574 0:e0ff0fa89d47 121 //ultrasonic 3
iainsc0574 1:1c796b8db63c 122 Bit1 = 0;
iainsc0574 1:1c796b8db63c 123 Bit2 = 1;
iainsc0574 1:1c796b8db63c 124 Bit3 = 0;
iainsc0574 1:1c796b8db63c 125 return;
iainsc0574 0:e0ff0fa89d47 126
iainsc0574 0:e0ff0fa89d47 127 case 3:
iainsc0574 0:e0ff0fa89d47 128 //ultrasonic 4
iainsc0574 1:1c796b8db63c 129 Bit1 = 1;
iainsc0574 1:1c796b8db63c 130 Bit2 = 1;
iainsc0574 1:1c796b8db63c 131 Bit3 = 0;
iainsc0574 1:1c796b8db63c 132 return;
iainsc0574 0:e0ff0fa89d47 133
iainsc0574 0:e0ff0fa89d47 134 case 4:
iainsc0574 0:e0ff0fa89d47 135 //ultrasonic 5
iainsc0574 1:1c796b8db63c 136 Bit1 = 0;
iainsc0574 1:1c796b8db63c 137 Bit2 = 0;
iainsc0574 1:1c796b8db63c 138 Bit3 = 1;
iainsc0574 1:1c796b8db63c 139 return;
iainsc0574 0:e0ff0fa89d47 140
iainsc0574 0:e0ff0fa89d47 141 }
iainsc0574 0:e0ff0fa89d47 142 }
iainsc0574 0:e0ff0fa89d47 143
iainsc0574 0:e0ff0fa89d47 144 int getPing(void){
iainsc0574 0:e0ff0fa89d47 145 int result=0;
iainsc0574 2:22d86efbbada 146 //start positive edge of trigger
iainsc0574 1:1c796b8db63c 147 Trig = HI;
iainsc0574 1:1c796b8db63c 148 US.reset();
iainsc0574 2:22d86efbbada 149 wait_us(10.0); //hold it high for 10us
iainsc0574 2:22d86efbbada 150 Trig = LO; //set negative going edge
iainsc0574 1:1c796b8db63c 151 while(Echo == 0){}; //while receive is LO
iainsc0574 1:1c796b8db63c 152 US.start(); //start counting received pulse
iainsc0574 1:1c796b8db63c 153 while(Echo == 1){};
iainsc0574 1:1c796b8db63c 154 US.stop();
iainsc0574 1:1c796b8db63c 155 result = ((US.read_us()*10)/58); //pulse duration = distance set to cm
iainsc0574 1:1c796b8db63c 156 return result;
iainsc0574 1:1c796b8db63c 157
iainsc0574 1:1c796b8db63c 158 }
iainsc0574 1:1c796b8db63c 159
iainsc0574 2:22d86efbbada 160 void turn(float s){
iainsc0574 2:22d86efbbada 161 s=s+1;
iainsc0574 2:22d86efbbada 162 if (s>=0 && s<=2) {
iainsc0574 2:22d86efbbada 163 steering.pulsewidth(s/2000+0.001);
iainsc0574 2:22d86efbbada 164 }
iainsc0574 2:22d86efbbada 165
iainsc0574 2:22d86efbbada 166 return;
iainsc0574 1:1c796b8db63c 167 }
iainsc0574 1:1c796b8db63c 168
iainsc0574 2:22d86efbbada 169 void drive(float v){
iainsc0574 2:22d86efbbada 170 v=v+1;
iainsc0574 2:22d86efbbada 171 if (v>=0 && v<=2) {
iainsc0574 2:22d86efbbada 172 //no reverse code
iainsc0574 2:22d86efbbada 173 velocity.pulsewidth(v/2000+0.001);
iainsc0574 2:22d86efbbada 174 vo=v;
iainsc0574 2:22d86efbbada 175 }
iainsc0574 1:1c796b8db63c 176 return;
iainsc0574 1:1c796b8db63c 177 }
iainsc0574 1:1c796b8db63c 178
iainsc0574 1:1c796b8db63c 179 void stop(void){
iainsc0574 2:22d86efbbada 180 drive(0.0);
iainsc0574 0:e0ff0fa89d47 181
iainsc0574 1:1c796b8db63c 182 return;
iainsc0574 1:1c796b8db63c 183 }
iainsc0574 1:1c796b8db63c 184
iainsc0574 2:22d86efbbada 185 int measurement_mean(int *Array){
iainsc0574 1:1c796b8db63c 186 int value=0;
iainsc0574 1:1c796b8db63c 187 int sum = 0;
iainsc0574 1:1c796b8db63c 188 for(int i=0;i<=MAX;i++){
iainsc0574 1:1c796b8db63c 189 sum +=Array[i];
iainsc0574 1:1c796b8db63c 190 }
iainsc0574 1:1c796b8db63c 191 value = sum/MAX;
iainsc0574 2:22d86efbbada 192 return (int)value;
iainsc0574 1:1c796b8db63c 193 }