Diff: main.cpp

Revision:

2:1549900755ba

Parent:

1:fc3a5c5100d2

--- a/main.cpp	Wed Nov 30 04:12:04 2011 +0000
+++ b/main.cpp	Wed Nov 30 19:17:21 2011 +0000
@@ -39,6 +39,7 @@
 DigitalOut valueLED1(p23);
 DigitalOut valueLED2(p25);
 
+DigitalOut Reset(p27);
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -135,6 +136,7 @@
     
     servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);
     servo_5.pulsewidth_us(1000 + (0 * 1000) / 90); 
+    servo_4.pulsewidth_us(0);
 
     Position1_1p = 1;
     Position2_1p = 0;
@@ -142,7 +144,11 @@
     Position2_2p = 0;
     
     state = 0;
+    Reset = 1;
     
+    valueLED1=0;
+    valueLED2=0;
+    clk=0;
     return;
 }           // end init_sys
 
@@ -206,15 +212,15 @@
                 command->result_data[1] = valueLED2;
             }
             else if((bSort_Mode == 1)&&(bMaint_Mode == 0)){  
-                command->nos_data = 7;// no data to be returned                 
+                command->nos_data = 2;// no data to be returned                 
                 command->result_data[0] = valueLED1;// counter1p; 
                 command->result_data[1] = valueLED2;// counter2p; 
-                command->result_data[2] = Motor;
+               /* command->result_data[2] = Motor;
                 command->result_data[3] = Position1_1p;
                 command->result_data[4] = Position2_1p;
                 command->result_data[5] = Position1_2p;
-                command->result_data[6] = Position2_2p;
-                send_data(&ext_cmd);
+                command->result_data[6] = Position2_2p;*/
+                //send_data(&ext_cmd);
             }
             break;
             
@@ -224,6 +230,7 @@
         case MAINT_MODE :
             bSort_Mode = 0;
             bMaint_Mode = 1;
+            Reset = 1;
             servo_4.pulsewidth_us(0); 
             servo_0.pulsewidth_us(0);
             servo_5.pulsewidth_us(0); 
@@ -238,17 +245,24 @@
         case SORT_MODE :
             bSort_Mode = 1;
             bMaint_Mode = 0;
-            lcd->cls();
+            Reset = 0;
+            state = 0;
+            lcd->cls(); 
             lcd->locate(0,0);
             lcd->printf("W3C");
             lcd->locate(1,0);
             lcd->printf("sort mode");
+            
             break;
             
 //
 // Urgency mode
 //            
         case URGENCY :
+            Reset = 1;
+            bSort_Mode = 0;
+            bMaint_Mode = 0;
+            state = 0;
             servo_4.pulsewidth_us(0); 
             servo_0.pulsewidth_us(0);
             servo_5.pulsewidth_us(0); 
@@ -262,6 +276,10 @@
 // Exit mode
 // 
         case EXIT :
+            Reset = 1;
+            bSort_Mode = 0;
+            bMaint_Mode = 0;
+            state = 0;
             servo_4.pulsewidth_us(0); 
             servo_0.pulsewidth_us(0);
             servo_5.pulsewidth_us(0); 
@@ -284,31 +302,44 @@
 
 //function to send value on the FPGA when 1p is detected
 void sensor1p (void){
+    sensor1.read();
+    sensor2.read();
+    
     clk = !clk;
     wait(0.01);
-    sensor1.read();
-        if(sensor1 > 0.8) {
+   
+        if(sensor1 > 0.5) {
                 led1 = 1;
-                if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
+                //if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
                     valueLED1 = 1;
-                }
+                //}
         }
-        else if(sensor1 < 0.8){
+        else if(sensor1 < 0.5){
                led1 = 0;
-               if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
+               //if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
                     valueLED1 = 0;
-               }
+               //}
         }
+
+        if(sensor2 > 0.5) {
+                led2 = 1;
+                valueLED2 = 1;
+        }
+        else if(sensor2 < 0.5){
+               led2 = 0;
+                valueLED2 = 0;
+        }
+        
 }
 
 //function to send value on the FPGA when 2p is detected
 void sensor2p (){
     sensor2.read();
-        if(sensor2 > 0.8) {
+        if(sensor2 > 0.5) {
                 led2 = 1;
                 valueLED2 = 1;
         }
-        else if(sensor2 < 0.8){
+        else if(sensor2 < 0.5){
                led2 = 0;
                 valueLED2 = 0;
         }
@@ -346,6 +377,7 @@
              break;
         case 1:                                                 // state 1 if counter1p = 1
              servo_4.pulsewidth_us(0);                          // motor stop
+             wait(2);
              servo_0.pulsewidth_us(1000 + (200 * 1000) / 90);   // servo 1p go to position 2 
              wait(1);
              Position1_1p = 0;
@@ -373,6 +405,7 @@
              break;
         case 4:                                                // state 4 if counter 2p = 1
              servo_4.pulsewidth_us(0);                         // motor stop
+             wait(2);
              servo_5.pulsewidth_us(1000 + (200 * 1000) / 90);  // servo 2p go to position 2
              wait(1);
              Position1_2p = 0;
@@ -406,16 +439,15 @@
 //************************************************************************
 //
 int main() {
-
-    valueLED1=0;
-    valueLED2=0;
-    clk=0;
+    
     init_sys();
+    Reset = 0;
     
     FOREVER {
     
-     timer2p.attach(&sensor2p, 0.1);         //function sensor2p is reading all the 0.1 ms
-     timer1p.attach(&sensor1p, 0.1);         //function sensor1p is reading all the 0.1 ms
+     timer1p.attach(&sensor1p, 0.02);         //function sensor1p is reading all the 30 ms
+   //  timer2p.attach(&sensor2p, 0.05);         //function sensor2p is reading all the 100 ms
+    
     
      
         counter1p.read();
@@ -426,7 +458,7 @@
     
 
      clk = !clk;
-     wait(0.01);
+     wait(0.001);
     
         get_cmd(&ext_cmd);
     //