UoD_ME40005_firefighting / Mbed 2 deprecated Ultrasonic_And_Motors

code using the sonic sensor to activate motor when the distance value dips beneath 50cm, seems to be a problem compiling with the else if statement. Apparently an unclosed bracket.

Dependencies:   SRF02 Servo mbed

main.cpp@3:732faa546abb, 2017-04-25 (annotated)

Committer:
mspeed93
Date:
Tue Apr 25 12:18:31 2017 +0000
Revision:
3:732faa546abb
Parent:
2:9179421b2288 
another format update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mspeed93 0:928ee2609d39 1 #include "mbed.h"
mspeed93 0:928ee2609d39 2 #include "SRF02.h"
mspeed93 0:928ee2609d39 3 #include "Servo.h"
mspeed93 0:928ee2609d39 4
mspeed93 1:b74e9a2a3fa5 5 //Serial pc(USBTX, USBRX); //tx, rx
mspeed93 1:b74e9a2a3fa5 6 SRF02 Fsensor(p28,p27, 0xE0); //front flame sensor
mspeed93 1:b74e9a2a3fa5 7 SRF02 Lsensor(p28,p27, 0xE4); //left flame sensor
mspeed93 1:b74e9a2a3fa5 8 SRF02 Rsensor(p28,p27, 0xE2); //right flame sensor
mspeed93 1:b74e9a2a3fa5 9 Servo cr_srv_L(p21); //left servo
mspeed93 1:b74e9a2a3fa5 10 Servo cr_srv_R(p22); //right servo
mspeed93 1:b74e9a2a3fa5 11 DigitalIn RFlame (p5); //right flame sensor
mspeed93 1:b74e9a2a3fa5 12 DigitalIn LFlame (p7); //left flame sensor
mspeed93 1:b74e9a2a3fa5 13 DigitalOut relay(p8); // Relay for controlling the fan
mspeed93 1:b74e9a2a3fa5 14 DigitalIn PUSH_BUTTON(p20); //push button to start program
mspeed93 1:b74e9a2a3fa5 15 DigitalOut myled(LED1); //on board led used to test functionality of systems
mspeed93 1:b74e9a2a3fa5 16
mspeed93 1:b74e9a2a3fa5 17
mspeed93 1:b74e9a2a3fa5 18 /* function that creates a start stop sequence for the main program by monitoring the state of the push button.
mspeed93 1:b74e9a2a3fa5 19 when the push button goes from 0 to 1 the value state of push button is updated and then the program breaks from the function and continues
mspeed93 1:b74e9a2a3fa5 20 with the rest of the program*/
mspeed93 2:9179421b2288 21
mspeed93 2:9179421b2288 22 void wait_for_one_press()
mspeed93 2:9179421b2288 23 {
mspeed93 1:b74e9a2a3fa5 24 int count = 0;
mspeed93 2:9179421b2288 25 int OLD_PUSH_BUTTON = 0;
mspeed93 1:b74e9a2a3fa5 26 int NEW_PUSH_BUTTON;
mspeed93 2:9179421b2288 27 while(1) {
mspeed93 2:9179421b2288 28 NEW_PUSH_BUTTON = PUSH_BUTTON;
mspeed93 2:9179421b2288 29 if ((NEW_PUSH_BUTTON == 1) && (OLD_PUSH_BUTTON ==0)) {
mspeed93 2:9179421b2288 30 count++;
mspeed93 2:9179421b2288 31 OLD_PUSH_BUTTON = NEW_PUSH_BUTTON;
mspeed93 2:9179421b2288 32 break ;
mspeed93 2:9179421b2288 33 }
mspeed93 1:b74e9a2a3fa5 34 }
mspeed93 1:b74e9a2a3fa5 35 }
mspeed93 0:928ee2609d39 36
mspeed93 2:9179421b2288 37 void init_servo()
mspeed93 2:9179421b2288 38 {
mspeed93 0:928ee2609d39 39 //calibrate the servos for +/-45deg
mspeed93 1:b74e9a2a3fa5 40 cr_srv_L.calibrate(0.0005,45);
mspeed93 1:b74e9a2a3fa5 41 cr_srv_R.calibrate(0.0005,45);
mspeed93 2:9179421b2288 42 }
mspeed93 2:9179421b2288 43
mspeed93 2:9179421b2288 44 void Forward_Drive()
mspeed93 2:9179421b2288 45 {
mspeed93 1:b74e9a2a3fa5 46 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 47 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 48 R_cr_srv_cmd = 0.465; //moderately fast clockwise rotation
mspeed93 2:9179421b2288 49 L_cr_srv_cmd = 0.535; //moderately fast anti-clockwise rotation
mspeed93 2:9179421b2288 50 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 2:9179421b2288 51 cr_srv_R.write(R_cr_srv_cmd); //writes the speed commands to the servos
mspeed93 2:9179421b2288 52 }
mspeed93 2:9179421b2288 53
mspeed93 2:9179421b2288 54 void Stop_Motors()
mspeed93 2:9179421b2288 55 {
mspeed93 1:b74e9a2a3fa5 56 float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 57 cr_srv_cmd = 0.5 ; //holds the motors still
mspeed93 2:9179421b2288 58 cr_srv_L.write(cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 59 cr_srv_R.write(cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 60 }
mspeed93 2:9179421b2288 61
mspeed93 2:9179421b2288 62 void Turn_left()
mspeed93 2:9179421b2288 63 {
mspeed93 1:b74e9a2a3fa5 64 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 65 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 66 R_cr_srv_cmd = 0.48; //moderate speed to control the angle of turn
mspeed93 2:9179421b2288 67 L_cr_srv_cmd = 0.48;
mspeed93 2:9179421b2288 68 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 2:9179421b2288 69 cr_srv_R.write(R_cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 70 }
mspeed93 2:9179421b2288 71
mspeed93 2:9179421b2288 72 void Turn_right()
mspeed93 2:9179421b2288 73 {
mspeed93 1:b74e9a2a3fa5 74 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 75 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 76 R_cr_srv_cmd = 0.53; //moderate speed to control the angle of turn
mspeed93 2:9179421b2288 77 L_cr_srv_cmd = 0.53;
mspeed93 2:9179421b2288 78 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 2:9179421b2288 79 cr_srv_R.write(R_cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 80 }
mspeed93 0:928ee2609d39 81
mspeed93 1:b74e9a2a3fa5 82
mspeed93 2:9179421b2288 83 void Turn_hard_right()
mspeed93 2:9179421b2288 84 {
mspeed93 1:b74e9a2a3fa5 85 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 86 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 87 HR_cr_srv_cmd = 0.6; //full speed to ensure the robot turns 90 degrees right
mspeed93 2:9179421b2288 88 HL_cr_srv_cmd = 0.6;
mspeed93 2:9179421b2288 89 cr_srv_L.write(HL_cr_srv_cmd);
mspeed93 2:9179421b2288 90 cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 91 }
mspeed93 1:b74e9a2a3fa5 92
mspeed93 2:9179421b2288 93 void Turn_hard_left()
mspeed93 2:9179421b2288 94 {
mspeed93 1:b74e9a2a3fa5 95 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 96 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 97 HR_cr_srv_cmd = 0.4; //full speed to ensure the robot turns 90 degrees left
mspeed93 2:9179421b2288 98 HL_cr_srv_cmd = 0.4;
mspeed93 2:9179421b2288 99 cr_srv_L.write(HL_cr_srv_cmd);
mspeed93 2:9179421b2288 100 cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
mspeed93 2:9179421b2288 101 }
mspeed93 1:b74e9a2a3fa5 102
mspeed93 2:9179421b2288 103 void Turning() //function to calculate the appropriate turning response
mspeed93 2:9179421b2288 104 {
mspeed93 2:9179421b2288 105 int Ldistance = Lsensor.getDistanceCm(); //uses the hard turning function to steer the robot clear of obstructions
mspeed93 2:9179421b2288 106 int Rdistance = Rsensor.getDistanceCm();
mspeed93 2:9179421b2288 107 if (Ldistance > Rdistance) {
mspeed93 2:9179421b2288 108 Turn_hard_left();
mspeed93 2:9179421b2288 109 }
mspeed93 2:9179421b2288 110
mspeed93 2:9179421b2288 111 else if (Rdistance > Ldistance) {
mspeed93 2:9179421b2288 112 Turn_hard_right();
mspeed93 2:9179421b2288 113 }
mspeed93 2:9179421b2288 114 }
mspeed93 2:9179421b2288 115
mspeed93 2:9179421b2288 116 /* Sub-system used to detect the presence, lock on to, and extinguish the fire*/
mspeed93 2:9179421b2288 117
mspeed93 2:9179421b2288 118 void Extinguish()
mspeed93 2:9179421b2288 119 {
mspeed93 2:9179421b2288 120
mspeed93 1:b74e9a2a3fa5 121 int on = 1,off = 0;
mspeed93 1:b74e9a2a3fa5 122
mspeed93 1:b74e9a2a3fa5 123 while((RFlame == 0) || (LFlame == 0)) {
mspeed93 1:b74e9a2a3fa5 124 int distanceF = Fsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 125 int distanceR = Rsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 126 int distanceL = Lsensor.getDistanceCm();
mspeed93 2:9179421b2288 127
mspeed93 2:9179421b2288 128 if ((RFlame == 0) && (LFlame == 0)) { //flame sensor outputs a voltage 0 when IR light is detected
mspeed93 2:9179421b2288 129 Forward_Drive();
mspeed93 2:9179421b2288 130 relay = on;
mspeed93 2:9179421b2288 131 } //activates the fan
mspeed93 2:9179421b2288 132 else if ((RFlame == 1) && (LFlame == 0)) { //No IR light detected input voltage = 1
mspeed93 2:9179421b2288 133 Turn_right(); //Turn towards the right to better align the fire with the other sensor
mspeed93 2:9179421b2288 134 relay = off;
mspeed93 2:9179421b2288 135 } else if ((RFlame == 0) && (LFlame == 1)) { //No IR light detected input voltage = 1
mspeed93 2:9179421b2288 136 Turn_left();
mspeed93 2:9179421b2288 137 relay = off;
mspeed93 2:9179421b2288 138 } else if ((RFlame == 1) && (LFlame == 1)) {
mspeed93 2:9179421b2288 139 relay = off;
mspeed93 2:9179421b2288 140 break;
mspeed93 2:9179421b2288 141 }
mspeed93 1:b74e9a2a3fa5 142 }
mspeed93 1:b74e9a2a3fa5 143 }
mspeed93 1:b74e9a2a3fa5 144
mspeed93 2:9179421b2288 145
mspeed93 2:9179421b2288 146
mspeed93 2:9179421b2288 147
mspeed93 2:9179421b2288 148
mspeed93 2:9179421b2288 149 int main()
mspeed93 2:9179421b2288 150 {
mspeed93 2:9179421b2288 151
mspeed93 0:928ee2609d39 152 float pos_val=0.5; //variable to hold posistion values
mspeed93 0:928ee2609d39 153 float step_size=0.01; //position increment/decrement value
mspeed93 0:928ee2609d39 154 float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 155 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 156 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 157 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 158 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 2:9179421b2288 159
mspeed93 1:b74e9a2a3fa5 160 int on = 1,off = 0;
mspeed93 2:9179421b2288 161
mspeed93 1:b74e9a2a3fa5 162 init_servo();
mspeed93 2:9179421b2288 163
mspeed93 2:9179421b2288 164
mspeed93 2:9179421b2288 165 wait_for_one_press(); //Waits at this line for push button to be activated
mspeed93 2:9179421b2288 166
mspeed93 2:9179421b2288 167
mspeed93 2:9179421b2288 168
mspeed93 2:9179421b2288 169 while(1) {
mspeed93 2:9179421b2288 170
mspeed93 3:732faa546abb 171 int Ldistance = Lsensor.getDistanceCm(); /reads the distance value the ultrasonic sensors detect
mspeed93 3:732faa546abb 172 int Rdistance = Rsensor.getDistanceCm(); // for front, left, and right sensors
mspeed93 2:9179421b2288 173 int Fdistance = Fsensor.getDistanceCm();
mspeed93 2:9179421b2288 174
mspeed93 2:9179421b2288 175
mspeed93 3:732faa546abb 176 if ((RFlame == 0) || (LFlame == 0)) { //conditional statement: if one or the other flame sensors detect
mspeed93 2:9179421b2288 177 Extinguish();
mspeed93 3:732faa546abb 178 } //fire then the extinguish function is called
mspeed93 2:9179421b2288 179
mspeed93 3:732faa546abb 180 if (Fdistance > 22) { //using the distance value in front forward drive or turning functions
mspeed93 3:732faa546abb 181 //are determined
mspeed93 2:9179421b2288 182 Forward_Drive();
mspeed93 2:9179421b2288 183 pos_val = pos_val + step_size;
mspeed93 2:9179421b2288 184
mspeed93 2:9179421b2288 185 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 2:9179421b2288 186
mspeed93 2:9179421b2288 187 wait(0.5);
mspeed93 2:9179421b2288 188 }
mspeed93 2:9179421b2288 189
mspeed93 3:732faa546abb 190 else if (Fdistance <= 22) { //activates turning function if front distance drops
mspeed93 3:732faa546abb 191 //beneath 22cm
mspeed93 2:9179421b2288 192 Turning();
mspeed93 2:9179421b2288 193 pos_val = pos_val + step_size;
mspeed93 2:9179421b2288 194 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 2:9179421b2288 195
mspeed93 2:9179421b2288 196 wait(0.5);
mspeed93 2:9179421b2288 197 }
mspeed93 2:9179421b2288 198
mspeed93 3:732faa546abb 199 if ((Ldistance <= 20) && (Fdistance >= 22)) { //measures the distance to the side of the robot and the front
mspeed93 3:732faa546abb 200 Turn_right(); //and turns away as appropriate using the slower turning functions
mspeed93 2:9179421b2288 201
mspeed93 2:9179421b2288 202 pos_val = pos_val + step_size;
mspeed93 2:9179421b2288 203 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 2:9179421b2288 204
mspeed93 2:9179421b2288 205 wait(0.5);
mspeed93 2:9179421b2288 206 }
mspeed93 2:9179421b2288 207
mspeed93 3:732faa546abb 208 else if ((Rdistance <= 20) && (Fdistance >= 22)) { //measures the distance to the side of the robot and the front
mspeed93 3:732faa546abb 209 Turn_left(); //and turns away as appropriate using the slower turning functions
mspeed93 2:9179421b2288 210
mspeed93 2:9179421b2288 211 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 2:9179421b2288 212
mspeed93 2:9179421b2288 213 wait(0.5);
mspeed93 2:9179421b2288 214 }
mspeed93 2:9179421b2288 215
mspeed93 2:9179421b2288 216
mspeed93 0:928ee2609d39 217 }
mspeed93 0:928ee2609d39 218 }