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@2:9179421b2288, 2017-04-25 (annotated)
- Committer:
- mspeed93
- Date:
- Tue Apr 25 12:15:22 2017 +0000
- Revision:
- 2:9179421b2288
- Parent:
- 1:b74e9a2a3fa5
- Child:
- 3:732faa546abb
Reformatted code;
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:9179421b2288 | 171 | int Ldistance = Lsensor.getDistanceCm(); //reads the distance value the ultrasonic sensors detect |
mspeed93 | 2:9179421b2288 | 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 | 2:9179421b2288 | 176 | if ((RFlame == 0) || (LFlame == 0)) { //conditional statement: if one or the other flame sensors detect |
mspeed93 | 2:9179421b2288 | 177 | Extinguish(); |
mspeed93 | 2:9179421b2288 | 178 | } //fire then the extinguish function is called |
mspeed93 | 2:9179421b2288 | 179 | |
mspeed93 | 2:9179421b2288 | 180 | if (Fdistance > 22) { //using the distance value in front forward drive or turning functions |
mspeed93 | 2:9179421b2288 | 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 | 2:9179421b2288 | 190 | else if (Fdistance <= 22) { //activates turning function if front distance drops |
mspeed93 | 2:9179421b2288 | 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 | 2:9179421b2288 | 199 | if ((Ldistance <= 20) && (Fdistance >= 22)) { //measures the distance to the side of the robot and the front |
mspeed93 | 2:9179421b2288 | 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 | 2:9179421b2288 | 208 | else if ((Rdistance <= 20) && (Fdistance >= 22)) { //measures the distance to the side of the robot and the front |
mspeed93 | 2:9179421b2288 | 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 | } |