Diff: main.cpp

Revision:

3:2441bcef5885

Parent:

2:e570143bf35e

Child:

4:d5502eb8adcd

--- a/main.cpp	Wed Jun 09 02:39:27 2021 +0000
+++ b/main.cpp	Wed Jun 09 10:36:41 2021 +0000
@@ -43,8 +43,8 @@
 DigitalOut eject_1 = A1; // valve 1
 DigitalOut eject_2 = A2;  // valve 2
 // DC motors
-DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; 轉夾
-DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; load 1 arrow
+DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
+DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; picker
 
 // Global Control variable
 char msg[200] = {0}; // msg for responser
@@ -119,21 +119,24 @@
         msg[strlen(msg)] = '0' + n_loaded;
         dc_motor1.set_out(0,1);    
         n_loaded++;  
-        if(firing2 == 1)
-        {
+        while(firing2 == 0);
+       // if(firing2 == 1)
+//        {
             if(n_loaded <= 5)
             {
-                dc_motor1.set_out(0,0.5);
-                wait(1.2);                
+                dc_motor1.set_out(0,0.7);
+                wait(1.2);     
+                dc_motor1.set_out(0,0);           
             }
             else
             {
                 strcpy(msg + len, "right arrow is pressed. ");
                 strcpy(msg + strlen(msg), "n_loaded cleared!");
                 n_loaded = 0;
+                load_manual = 1;
             }
-            dc_motor1.set_out(0,0);
-        }
+            
+//        }
 //        load_manual = 1;
         break;
     case -6:
@@ -246,7 +249,7 @@
     
     // Initiate firing pins
     firing1.mode(PullUp); // default 1
-    firing2.mode(PullDown); //default 0
+    firing2.mode(PullUp); // default 1
     firing3.mode(PullUp); // default 1
     
     // Reset dc motor
@@ -254,10 +257,10 @@
     dc_motor1.set_pid(0.008, 0, 0, 0.0);
     dc_motor2.reset();
     dc_motor2.set_pid(0.008, 0, 0, 0.0);
-//    dc_motor2.set_out(0,0.5);
+//    dc_motor2.set_out(0,0.5);  //picker
 //    wait(0.3);
 //    dc_motor2.move_angle(100);
-    dc_motor2.set_out(0,0);
+//    dc_motor2.set_out(0,1);
 //     dc_motor1.move_angle(100);
 //    dc_motor1.set_out(0.5,0); // -> testing only
     
@@ -275,7 +278,7 @@
         if(button == 1)
         {
             myled = 0;
-            if(!faul_reset)
+            if(!faul_reset && firing1 == 0)
             {
                 down = 0; up = 1; back = 1; // 101
                 start = 1;
@@ -283,30 +286,38 @@
             }
             else
             {
-                dc_motor1.set_out(0,1);   
+                dc_motor1.set_out(0,1); 
+                load_manual = 1;  
             }
             while(button == 1);
             wait(0.2);
             memset(msg, 0, sizeof(msg));
             strcpy(msg, "user button is pressed, faul_reset is ");
-            msg[strlen(msg)] = '0' + faul_reset;            
+            msg[strlen(msg)] = '0' + faul_reset;           
+            strcpy(msg + strlen(msg), ", firing2 is ");
+            msg[strlen(msg)] = '0' + firing2.read();
+            strcpy(msg + strlen(msg), ", load_manual is ");
+            msg[strlen(msg)] = '0' + load_manual;
             responser.publish(&str_msg);
         }
         if(load_manual && firing2 == 1)
         {
-            dc_motor1.set_out(0,0);   
+            wait(0.17);
+            dc_motor1.set_out(0,0);  
+            memset(msg, 0, sizeof(msg));
+            strcpy(msg, "firing2 is touched: ");
+            msg[strlen(msg)] = '0' + firing2.read();             
         }
-        if(firing1.read() == 0 && start == 1)
+        if(firing1.read() == 1 && start == 1)
         {
             up = 0; down = 1; back = 1; // 110
-            while(firing1.read() == 0);
             wait(0.3);
-            down = 0; back = 0; up = 0;
-            start = 0;
             memset(msg, 0, sizeof(msg));
             strcpy(msg, "firing1 is touched: ");
             msg[strlen(msg)] = '0' + firing1.read();
             responser.publish(&str_msg);
+            down = 0; back = 0; up = 0;
+            start = 0;
             
         }
     }