Diff: main.cpp

Revision:

4:d5502eb8adcd

Parent:

3:2441bcef5885

Child:

5:03af248f3f7c

--- a/main.cpp	Wed Jun 09 10:36:41 2021 +0000
+++ b/main.cpp	Fri Jun 11 02:19:26 2021 +0000
@@ -96,7 +96,7 @@
         break;
     case 4:
         strcpy(msg + len, "square is pressed");
-        up = 1; down = 1;
+        up = 1; down = 1; lock = 1;
         break;
     case -4:
         strcpy(msg + len, "square is released");
@@ -120,24 +120,22 @@
         dc_motor1.set_out(0,1);    
         n_loaded++;  
         while(firing2 == 0);
-       // if(firing2 == 1)
-//        {
-            if(n_loaded <= 5)
-            {
-                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;
-            }
+
+        if(n_loaded <= 5)
+        {
+            dc_motor1.set_out(0,0.7);
+            wait(1.4);     
+            dc_motor1.set_out(0,0);           
+        }
+        else
+        {
+            load_manual = 1;
+            strcpy(msg + len, "right arrow is pressed. ");
+            strcpy(msg + strlen(msg), "n_loaded cleared!");
+            n_loaded = 0;
             
-//        }
-//        load_manual = 1;
+        }
+            
         break;
     case -6:
         
@@ -257,13 +255,7 @@
     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);  //picker
-//    wait(0.3);
-//    dc_motor2.move_angle(100);
-//    dc_motor2.set_out(0,1);
-//     dc_motor1.move_angle(100);
-//    dc_motor1.set_out(0.5,0); // -> testing only
-    
+    up = 1; back = 1; down = 0;
     // Main programming
     int start = 0; // control variable for resetting shooter
     int tmp = 0;
@@ -279,10 +271,15 @@
         {
             myled = 0;
             if(!faul_reset && firing1 == 0)
+//            if(1)
             {
                 down = 0; up = 1; back = 1; // 101
                 start = 1;
-                faul_reset = 1;
+                memset(msg, 0, sizeof(msg));
+                strcpy(msg, "Start resetting Faulhaber: firing1 is ");
+                msg[strlen(msg)] = '0' + firing1.read();
+                responser.publish(&str_msg);
+//                faul_reset = 1;
             }
             else
             {