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@1:b74e9a2a3fa5, 2017-04-24 (annotated)

Committer:
mspeed93
Date:
Mon Apr 24 22:56:38 2017 +0000
Revision:
1:b74e9a2a3fa5
Parent:
0:928ee2609d39
Child:
2:9179421b2288
Full robot code

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 1:b74e9a2a3fa5 21
mspeed93 1:b74e9a2a3fa5 22 void wait_for_one_press(){
mspeed93 1:b74e9a2a3fa5 23 int count = 0;
mspeed93 1:b74e9a2a3fa5 24 int OLD_PUSH_BUTTON = 0;
mspeed93 1:b74e9a2a3fa5 25 int NEW_PUSH_BUTTON;
mspeed93 1:b74e9a2a3fa5 26 while(1){
mspeed93 1:b74e9a2a3fa5 27 NEW_PUSH_BUTTON = PUSH_BUTTON;
mspeed93 1:b74e9a2a3fa5 28 if ((NEW_PUSH_BUTTON == 1) && (OLD_PUSH_BUTTON ==0)){
mspeed93 1:b74e9a2a3fa5 29 count++;
mspeed93 1:b74e9a2a3fa5 30 OLD_PUSH_BUTTON = NEW_PUSH_BUTTON;
mspeed93 1:b74e9a2a3fa5 31 break ;}
mspeed93 1:b74e9a2a3fa5 32 }
mspeed93 1:b74e9a2a3fa5 33 }
mspeed93 0:928ee2609d39 34
mspeed93 0:928ee2609d39 35 void init_servo(){
mspeed93 0:928ee2609d39 36 //calibrate the servos for +/-45deg
mspeed93 1:b74e9a2a3fa5 37 cr_srv_L.calibrate(0.0005,45);
mspeed93 1:b74e9a2a3fa5 38 cr_srv_R.calibrate(0.0005,45);
mspeed93 0:928ee2609d39 39 }
mspeed93 1:b74e9a2a3fa5 40
mspeed93 1:b74e9a2a3fa5 41 void Forward_Drive(){
mspeed93 1:b74e9a2a3fa5 42 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 43 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 44 R_cr_srv_cmd = 0.465; //moderately fast clockwise rotation
mspeed93 1:b74e9a2a3fa5 45 L_cr_srv_cmd = 0.535; //moderately fast anti-clockwise rotation
mspeed93 1:b74e9a2a3fa5 46 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 1:b74e9a2a3fa5 47 cr_srv_R.write(R_cr_srv_cmd); //writes the speed commands to the servos
mspeed93 1:b74e9a2a3fa5 48 }
mspeed93 1:b74e9a2a3fa5 49
mspeed93 1:b74e9a2a3fa5 50 void Stop_Motors(){
mspeed93 1:b74e9a2a3fa5 51 float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 52 cr_srv_cmd = 0.5 ; //holds the motors still
mspeed93 1:b74e9a2a3fa5 53 cr_srv_L.write(cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 54 cr_srv_R.write(cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 55 }
mspeed93 1:b74e9a2a3fa5 56
mspeed93 1:b74e9a2a3fa5 57 void Turn_left(){
mspeed93 1:b74e9a2a3fa5 58 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 59 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 60 R_cr_srv_cmd = 0.48; //moderate speed to control the angle of turn
mspeed93 1:b74e9a2a3fa5 61 L_cr_srv_cmd = 0.48;
mspeed93 1:b74e9a2a3fa5 62 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 1:b74e9a2a3fa5 63 cr_srv_R.write(R_cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 64 }
mspeed93 1:b74e9a2a3fa5 65
mspeed93 1:b74e9a2a3fa5 66 void Turn_right(){
mspeed93 1:b74e9a2a3fa5 67 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 68 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 69 R_cr_srv_cmd = 0.53; //moderate speed to control the angle of turn
mspeed93 1:b74e9a2a3fa5 70 L_cr_srv_cmd = 0.53;
mspeed93 1:b74e9a2a3fa5 71 cr_srv_L.write(L_cr_srv_cmd);
mspeed93 1:b74e9a2a3fa5 72 cr_srv_R.write(R_cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 73 }
mspeed93 0:928ee2609d39 74
mspeed93 1:b74e9a2a3fa5 75
mspeed93 1:b74e9a2a3fa5 76 void Turn_hard_right(){
mspeed93 1:b74e9a2a3fa5 77 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 78 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 79 HR_cr_srv_cmd = 0.6; //full speed to ensure the robot turns 90 degrees right
mspeed93 1:b74e9a2a3fa5 80 HL_cr_srv_cmd = 0.6;
mspeed93 1:b74e9a2a3fa5 81 cr_srv_L.write(HL_cr_srv_cmd);
mspeed93 1:b74e9a2a3fa5 82 cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 83 }
mspeed93 1:b74e9a2a3fa5 84
mspeed93 1:b74e9a2a3fa5 85 void Turn_hard_left(){
mspeed93 1:b74e9a2a3fa5 86 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 87 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 88 HR_cr_srv_cmd = 0.4; //full speed to ensure the robot turns 90 degrees left
mspeed93 1:b74e9a2a3fa5 89 HL_cr_srv_cmd = 0.4;
mspeed93 1:b74e9a2a3fa5 90 cr_srv_L.write(HL_cr_srv_cmd);
mspeed93 1:b74e9a2a3fa5 91 cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
mspeed93 1:b74e9a2a3fa5 92 }
mspeed93 1:b74e9a2a3fa5 93
mspeed93 1:b74e9a2a3fa5 94 void Turning(){ //function to calculate the appropriate turning response
mspeed93 1:b74e9a2a3fa5 95 int Ldistance = Lsensor.getDistanceCm(); //uses the hard turning function to steer the robot clear of obstructions
mspeed93 1:b74e9a2a3fa5 96 int Rdistance = Rsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 97 if (Ldistance > Rdistance){
mspeed93 1:b74e9a2a3fa5 98 Turn_hard_left();
mspeed93 1:b74e9a2a3fa5 99 }
mspeed93 1:b74e9a2a3fa5 100
mspeed93 1:b74e9a2a3fa5 101 else if (Rdistance > Ldistance){
mspeed93 1:b74e9a2a3fa5 102 Turn_hard_right();
mspeed93 1:b74e9a2a3fa5 103 }
mspeed93 1:b74e9a2a3fa5 104 }
mspeed93 1:b74e9a2a3fa5 105
mspeed93 1:b74e9a2a3fa5 106 /* Sub-system used to detect the presence, lock on to the fire, and extinguish it*/
mspeed93 1:b74e9a2a3fa5 107
mspeed93 1:b74e9a2a3fa5 108 void Extinguish(){
mspeed93 1:b74e9a2a3fa5 109
mspeed93 1:b74e9a2a3fa5 110 int on = 1,off = 0;
mspeed93 1:b74e9a2a3fa5 111
mspeed93 1:b74e9a2a3fa5 112
mspeed93 1:b74e9a2a3fa5 113 // Loop forever, relay on/ off for 1 second each
mspeed93 1:b74e9a2a3fa5 114 // LED on relay will blink on / off
mspeed93 1:b74e9a2a3fa5 115 while((RFlame == 0) || (LFlame == 0)) {
mspeed93 1:b74e9a2a3fa5 116 int distanceF = Fsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 117 int distanceR = Rsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 118 int distanceL = Lsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 119
mspeed93 1:b74e9a2a3fa5 120 if ((RFlame == 0) && (LFlame == 0)){ //flame sensor outputs a voltage 0 when IR light is detected
mspeed93 1:b74e9a2a3fa5 121 Forward_Drive();
mspeed93 1:b74e9a2a3fa5 122 relay = on;} //activates the fan
mspeed93 1:b74e9a2a3fa5 123 else if ((RFlame == 1) && (LFlame == 0)){ //No IR light detected input voltage = 1
mspeed93 1:b74e9a2a3fa5 124 Turn_right(); //Turn towards the right to better align the fire with the other sensor
mspeed93 1:b74e9a2a3fa5 125 relay = off;}
mspeed93 1:b74e9a2a3fa5 126 else if ((RFlame == 0) && (LFlame == 1)){ //No IR light detected input voltage = 1
mspeed93 1:b74e9a2a3fa5 127 Turn_left();
mspeed93 1:b74e9a2a3fa5 128 relay = off;}
mspeed93 1:b74e9a2a3fa5 129 else if ((RFlame == 1) && (LFlame == 1)){
mspeed93 1:b74e9a2a3fa5 130 relay = off;
mspeed93 1:b74e9a2a3fa5 131 break;}
mspeed93 1:b74e9a2a3fa5 132 }
mspeed93 1:b74e9a2a3fa5 133 }
mspeed93 1:b74e9a2a3fa5 134
mspeed93 1:b74e9a2a3fa5 135
mspeed93 1:b74e9a2a3fa5 136
mspeed93 1:b74e9a2a3fa5 137
mspeed93 1:b74e9a2a3fa5 138
mspeed93 0:928ee2609d39 139 int main() {
mspeed93 0:928ee2609d39 140
mspeed93 0:928ee2609d39 141 float pos_val=0.5; //variable to hold posistion values
mspeed93 0:928ee2609d39 142 float step_size=0.01; //position increment/decrement value
mspeed93 0:928ee2609d39 143 float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 144 float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 145 float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 146 float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 1:b74e9a2a3fa5 147 float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
mspeed93 0:928ee2609d39 148
mspeed93 1:b74e9a2a3fa5 149 int on = 1,off = 0;
mspeed93 1:b74e9a2a3fa5 150
mspeed93 1:b74e9a2a3fa5 151 init_servo();
mspeed93 1:b74e9a2a3fa5 152
mspeed93 1:b74e9a2a3fa5 153
mspeed93 1:b74e9a2a3fa5 154 wait_for_one_press();
mspeed93 1:b74e9a2a3fa5 155
mspeed93 1:b74e9a2a3fa5 156
mspeed93 0:928ee2609d39 157
mspeed93 0:928ee2609d39 158 while(1){
mspeed93 0:928ee2609d39 159
mspeed93 1:b74e9a2a3fa5 160 int Ldistance = Lsensor.getDistanceCm(); //reads the distance value the ultrasonic sensors detect
mspeed93 1:b74e9a2a3fa5 161 int Rdistance = Rsensor.getDistanceCm(); // for front, left, and right sensors
mspeed93 1:b74e9a2a3fa5 162 int Fdistance = Fsensor.getDistanceCm();
mspeed93 1:b74e9a2a3fa5 163
mspeed93 0:928ee2609d39 164
mspeed93 1:b74e9a2a3fa5 165 if ((RFlame == 0) || (LFlame == 0)){ //conditional statement: if one or the other flame sensors detect
mspeed93 1:b74e9a2a3fa5 166 Extinguish();} //fire then the extinguish function is called
mspeed93 1:b74e9a2a3fa5 167
mspeed93 1:b74e9a2a3fa5 168 if (Fdistance > 22){ //using the distance value in front forward drive or turning functions
mspeed93 1:b74e9a2a3fa5 169 //are determined
mspeed93 1:b74e9a2a3fa5 170 Forward_Drive();
mspeed93 1:b74e9a2a3fa5 171 pos_val = pos_val + step_size;
mspeed93 1:b74e9a2a3fa5 172
mspeed93 1:b74e9a2a3fa5 173 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 1:b74e9a2a3fa5 174
mspeed93 1:b74e9a2a3fa5 175 wait(0.5);}
mspeed93 1:b74e9a2a3fa5 176
mspeed93 1:b74e9a2a3fa5 177 else if (Fdistance <= 22){ //activates turning function if front distance drops
mspeed93 1:b74e9a2a3fa5 178 //beneath 22cm
mspeed93 1:b74e9a2a3fa5 179 Turning();
mspeed93 0:928ee2609d39 180 pos_val = pos_val + step_size;
mspeed93 0:928ee2609d39 181 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 0:928ee2609d39 182
mspeed93 1:b74e9a2a3fa5 183 wait(0.5);}
mspeed93 1:b74e9a2a3fa5 184
mspeed93 1:b74e9a2a3fa5 185 if ((Ldistance <= 20) && (Fdistance >= 22)){ //measures the distance to the side of the robot and the front
mspeed93 1:b74e9a2a3fa5 186 Turn_right(); //and turns away as appropriate using the slower turning functions
mspeed93 1:b74e9a2a3fa5 187
mspeed93 1:b74e9a2a3fa5 188 pos_val = pos_val + step_size;
mspeed93 0:928ee2609d39 189 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 0:928ee2609d39 190
mspeed93 1:b74e9a2a3fa5 191 wait(0.5);}
mspeed93 0:928ee2609d39 192
mspeed93 1:b74e9a2a3fa5 193 else if ((Rdistance <= 20) && (Fdistance >= 22)){ //measures the distance to the side of the robot and the front
mspeed93 1:b74e9a2a3fa5 194 Turn_left(); //and turns away as appropriate using the slower turning functions
mspeed93 1:b74e9a2a3fa5 195
mspeed93 1:b74e9a2a3fa5 196 if (pos_val > 1.0) pos_val = 0.0;
mspeed93 1:b74e9a2a3fa5 197
mspeed93 1:b74e9a2a3fa5 198 wait(0.5);}
mspeed93 1:b74e9a2a3fa5 199
mspeed93 1:b74e9a2a3fa5 200
mspeed93 0:928ee2609d39 201 }
mspeed93 0:928ee2609d39 202 }