Diff: main.cpp

Revision:

1:b74e9a2a3fa5

Parent:

0:928ee2609d39

Child:

2:9179421b2288

--- a/main.cpp	Thu Feb 16 22:41:28 2017 +0000
+++ b/main.cpp	Mon Apr 24 22:56:38 2017 +0000
@@ -2,45 +2,201 @@
 #include "SRF02.h"
 #include "Servo.h"
 
-Serial pc(USBTX, USBRX); //tx, rx
-SRF02 sensor(p28,p27, 0xE0);
-Servo cr_srv(p21);
-DigitalOut myled(LED1);
+//Serial pc(USBTX, USBRX); //tx, rx
+SRF02 Fsensor(p28,p27, 0xE0); //front flame sensor
+SRF02 Lsensor(p28,p27, 0xE4); //left flame sensor
+SRF02 Rsensor(p28,p27, 0xE2); //right flame sensor
+Servo cr_srv_L(p21); //left servo
+Servo cr_srv_R(p22); //right servo
+DigitalIn RFlame (p5); //right flame sensor
+DigitalIn LFlame (p7); //left flame sensor
+DigitalOut relay(p8); // Relay for controlling the fan
+DigitalIn PUSH_BUTTON(p20); //push button to start program
+DigitalOut myled(LED1); //on board led used to test functionality of systems
+
+
+/* function that creates a start stop sequence for the main program by monitoring the state of the push button.
+    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
+    with the rest of the program*/
+    
+void wait_for_one_press(){
+    int count = 0;
+    int OLD_PUSH_BUTTON = 0;                                  
+    int NEW_PUSH_BUTTON;
+    while(1){
+      NEW_PUSH_BUTTON = PUSH_BUTTON;
+      if ((NEW_PUSH_BUTTON == 1) && (OLD_PUSH_BUTTON ==0)){
+          count++;
+          OLD_PUSH_BUTTON = NEW_PUSH_BUTTON;
+          break ;}          
+    }
+}
 
 void init_servo(){
     //calibrate the servos for +/-45deg
-    cr_srv.calibrate(0.0005,45);
+    cr_srv_L.calibrate(0.0005,45);
+    cr_srv_R.calibrate(0.0005,45);
     }
+    
+void Forward_Drive(){
+    float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    R_cr_srv_cmd = 0.465;   //moderately fast clockwise rotation
+     L_cr_srv_cmd = 0.535;  //moderately fast anti-clockwise rotation
+     cr_srv_L.write(L_cr_srv_cmd);
+     cr_srv_R.write(R_cr_srv_cmd); //writes the speed commands to the servos
+     }
+     
+void Stop_Motors(){
+    float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    cr_srv_cmd = 0.5 ; //holds the motors still
+     cr_srv_L.write(cr_srv_cmd); //write to the servo
+     cr_srv_R.write(cr_srv_cmd); //write to the servo
+     }
+     
+void Turn_left(){
+    float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    R_cr_srv_cmd = 0.48;    //moderate speed to control the angle of turn
+     L_cr_srv_cmd = 0.48;
+     cr_srv_L.write(L_cr_srv_cmd);
+     cr_srv_R.write(R_cr_srv_cmd); //write to the servo
+     }
+     
+void Turn_right(){
+    float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    R_cr_srv_cmd = 0.53;    //moderate speed to control the angle of turn
+     L_cr_srv_cmd = 0.53;
+     cr_srv_L.write(L_cr_srv_cmd);
+     cr_srv_R.write(R_cr_srv_cmd); //write to the servo
+     }
 
+
+void Turn_hard_right(){
+    float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    HR_cr_srv_cmd = 0.6;     //full speed to ensure the robot turns 90 degrees right
+     HL_cr_srv_cmd = 0.6;
+     cr_srv_L.write(HL_cr_srv_cmd);
+     cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
+     }
+
+void Turn_hard_left(){
+    float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    HR_cr_srv_cmd = 0.4;     //full speed to ensure the robot turns 90 degrees left
+     HL_cr_srv_cmd = 0.4;
+     cr_srv_L.write(HL_cr_srv_cmd);
+     cr_srv_R.write(HR_cr_srv_cmd); //write to the servo
+     }
+
+void Turning(){                                      //function to calculate the appropriate turning response
+     int Ldistance = Lsensor.getDistanceCm();        //uses the hard turning function to steer the robot clear of obstructions
+     int Rdistance = Rsensor.getDistanceCm();
+     if (Ldistance > Rdistance){
+         Turn_hard_left();
+         }
+         
+         else if (Rdistance > Ldistance){
+             Turn_hard_right();
+             }      
+     }
+     
+     /* Sub-system used to detect the presence, lock on to the fire, and extinguish it*/
+     
+void Extinguish(){
+    
+    int on = 1,off = 0;
+
+ 
+    // Loop forever, relay on/ off for 1 second each
+    // LED on relay will blink on / off 
+    while((RFlame == 0) || (LFlame == 0)) {
+        int distanceF = Fsensor.getDistanceCm();
+        int distanceR = Rsensor.getDistanceCm();
+        int distanceL = Lsensor.getDistanceCm();
+        
+        if ((RFlame == 0) && (LFlame == 0)){      //flame sensor outputs a voltage 0 when IR light is detected 
+       Forward_Drive();
+       relay = on;}                              //activates the fan
+       else if ((RFlame == 1) && (LFlame == 0)){ //No IR light detected input voltage = 1 
+       Turn_right();                        //Turn towards the right to better align the fire with the other sensor
+       relay = off;}
+       else if ((RFlame == 0) && (LFlame == 1)){ //No IR light detected input voltage = 1 
+       Turn_left();
+       relay = off;}
+       else if ((RFlame == 1) && (LFlame == 1)){
+           relay = off;
+           break;}
+    }
+}
+
+     
+     
+     
+     
 int main() {
    
     float pos_val=0.5; //variable to hold posistion values
     float step_size=0.01; //position increment/decrement value
     float cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float R_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float L_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float HR_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
+    float HL_cr_srv_cmd=0.5; //continuous rotation servo command value (range: 0.0 - 1.0)
     
-    pc.baud(921600);
+    int on = 1,off = 0;
+    
+    init_servo();
+    
+    
+    wait_for_one_press();
+    
+    
     
    while(1){
       
-     int distance = sensor.getDistanceCm();
-     pc.printf("Distance = %d cm\n",distance);
+       int Ldistance = Lsensor.getDistanceCm();    //reads the distance value the ultrasonic sensors detect
+       int Rdistance = Rsensor.getDistanceCm();    // for front, left, and right sensors 
+       int Fdistance = Fsensor.getDistanceCm();
+    
      
-     if (distance <= 50)
-     myled = 0;
-     cr_srv_cmd = 0.5; //use a smaller scale for the cr servo due to its limited sensitivity
-     cr_srv.write(cr_srv_cmd); //write to the servo
+     if ((RFlame == 0) || (LFlame == 0)){         //conditional statement: if one or the other flame sensors detect 
+    Extinguish();}                                //fire then the extinguish function is called
+    
+     if  (Fdistance > 22){                        //using the distance value in front forward drive or turning functions 
+                                                  //are determined
+         Forward_Drive();
+     pos_val = pos_val + step_size;
+     
+        if (pos_val > 1.0) pos_val = 0.0;
+        
+        wait(0.5);} 
+            
+     else if (Fdistance <= 22){                                     //activates turning function if front distance drops
+                                                                    //beneath 22cm
+         Turning();
      pos_val = pos_val + step_size;
         if (pos_val > 1.0) pos_val = 0.0;
         
-        wait(0.5);
-     else if (distance > 50)
-     myled = 1;
-     cr_srv_cmd = 0.9;
-     cr_srv.write(cr_srv_cmd);
-     pos_val = pos_val + step_size;
+     wait(0.5);}
+     
+      if ((Ldistance <= 20) && (Fdistance >= 22)){                  //measures the distance to the side of the robot and the front
+             Turn_right();                                          //and turns away as appropriate using the slower turning functions
+             
+             pos_val = pos_val + step_size;
         if (pos_val > 1.0) pos_val = 0.0;
         
-     wait(0.5);
+     wait(0.5);}
      
+     else if ((Rdistance <= 20) && (Fdistance >= 22)){              //measures the distance to the side of the robot and the front
+             Turn_left();                                           //and turns away as appropriate using the slower turning functions
+             
+        if (pos_val > 1.0) pos_val = 0.0;
+        
+     wait(0.5);}
+     
+         
     }
 }
\ No newline at end of file