Diff: main.cpp

Revision:

6:0b79b90b83a2

Parent:

5:03af248f3f7c

Child:

7:66c6e14a1d97

Child:

8:9abe58bd7c07

--- a/main.cpp	Mon Jun 14 09:24:44 2021 +0000
+++ b/main.cpp	Wed Jun 16 10:10:57 2021 +0000
@@ -23,6 +23,7 @@
 // Subs
 ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
 ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
+//ros::Subscriber<std_msgs::Int16> sub_cam("PotCoordinate", msgCam);
 ros::Subscriber<std_msgs::Int16> sub_cam("PotDistance", msgCam);
 // Pubs
 std_msgs::String str_msg;
@@ -68,20 +69,44 @@
 // cam callback
 void msgCam(const std_msgs::Int16 &cam_msg)
 {
-    int minimum = 250;
-    int maximum = 350;
-//    int data = cam_msg.data;
-    if(auto_shooting > 0)
+    float minimum = 250;
+    float maximum = 600;
+    /*
+    if( cam_msg.data > 0)
+    {
+        aout = (maximum - float(cam_msg.data)) / (maximum - minimum);
+        if(auto_shooting)
+        {
+            memset(msg2, 0, sizeof(msg2));
+            strcpy(msg2, "Cam distance: ");
+            itoa(cam_msg.data, msg2 + strlen(msg2), 10);
+            strcpy(msg2 + strlen(msg2), ", aout(1000): ");
+            itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
+    //        sprintf("Cam distance: %d, %.4f", data, aout.read());
+            responser2.publish(&str_msg2);
+        }
+    }*/
+    if( cam_msg.data > 0 && auto_shooting)
     {
-        aout = (cam_msg.data - minimum) / (maximum - minimum);
-        memset(msg2, 0, sizeof(msg2));
-        strcpy(msg2, "Cam distance: ");
-        itoa(cam_msg.data, msg2 + strlen(msg2), 10);
-        strcpy(msg2 + strlen(msg2), ", aout(*1000): ");
-        itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
-//        sprintf("Cam distance: %d, %.4f", data, aout.read());
-        responser2.publish(&str_msg2);
+        int outValue[3] = [cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10];
+        if(auto_shooting)
+        {
+            memset(msg2, 0, sizeof(msg2));
+            strcpy(msg2, "Cam distance: ");
+            itoa(cam_msg.data, msg2 + strlen(msg2), 10);
+            strcpy(msg2 + strlen(msg2), ", aout(1000): ");
+            itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
+    //        sprintf("Cam distance: %d, %.4f", data, aout.read());
+            responser2.publish(&str_msg2);
+            for(int i = 0; i<3; i++){
+                outValue[i]/8;
+                (outValue[i]/4)%2;
+                (outValue[i]/2
+                (outValue[i]%2;
+            }
+        }
     }
+    
 }
 
 // button_msg: Int8
@@ -141,8 +166,6 @@
         
         pin1 = 0;
         pin3 = 0;
-        if(smooth)
-            pin4 = 1;
         pin2 = 1;
         strcpy(msg + len, "down arrow is pressed, pin2 is ");
         msg[strlen(msg)] = '0' + int(pin2);
@@ -175,34 +198,34 @@
         }
             
         break;
-    case -6:
-        
+    case -6:        
         strcpy(msg + len, "right arrow is released. ");
         break;
-    case 7:
-        
+    case 7:        
         pin2 = 0;
-        if(smooth)
-            pin4 = 1;
         pin1 = 1;
         strcpy(msg + len, "up arrow is pressed, pin1 is");
         msg[strlen(msg)] = '0' + int(pin1);
         break;
     case -7:
-        pin1 = 0;
+        pin1 = 0; 
         strcpy(msg + len, "up arrow is released, pin1 is");
         msg[strlen(msg)] = '0' + int(pin1);
         break;
     case 8:
-        strcpy(msg + len, "left arrow is pressed, left is ");
+        strcpy(msg + len, "left arrow is pressed, phase is ");
         msg[strlen(msg)] = '0' + stage_pick;
         if(stage_pick == 0)
         {
-            dc_motor2.set_out(0, 0.5);
-            if(firing3 == 1)
-                dc_motor2.set_out(0,0);
+            dc_motor2.set_out(0, 1);
+//            if(firing3 == 1)
+//            {
+//                dc_motor2.set_out(0,0);
+//                strcpy(msg + strlen(msg), ", firing pin detected. ");
+//            }
             stage_pick++;
             eject_1 = 1; eject_2 = 0;
+            strcpy(msg + strlen(msg), "rotate out");
         }
         else if(stage_pick == 1)
         {
@@ -210,6 +233,7 @@
             wait(0.3);
             dc_motor2.set_out(0,0);
             stage_pick++;
+            strcpy(msg + strlen(msg), "rotate phase 1");
         }
         else if(stage_pick == 2)
         {
@@ -217,41 +241,37 @@
             wait(0.3);
             dc_motor2.set_out(0,0);
             stage_pick = 0;
+            strcpy(msg + strlen(msg), "rotate phase 2");
         }
         break;
     case -8:
-        strcpy(msg + len, "left arrow is released, left is ");
+//        strcpy(msg + len, "left arrow is released, left is ");
         break;
     case 9:
-        auto_shooting = 1 - auto_shooting;
-        if(auto_shooting)
-        {
-            pin4 = 1; pin3 = 1; pin1 = 0; pin2 = 0;
-        }
-        else
-        {
-            pin3 = 0; pin4 = 0; // dun change order, in case delay           
-        }
-        
-        strcpy(msg + len, "L1 is pressed, auto_shooting is ");
-        msg[strlen(msg)] = '0' + auto_shooting;
+        strcpy(msg + len, "L1 is pressed");
+//        msg[strlen(msg)] = '0' + auto_shooting;
         break;
     case -9:
+//        pin3 = 0; pin4 = 0;
         strcpy(msg + len, "L1 is released");
         break;
     case 10:
+        
         dc_motor1.set_out(0,0);
         dc_motor2.set_out(0,0);
-        strcpy(msg + len, "R1 is pressed");
+        auto_shooting = 1;
+        pin4 = 1; 
+        strcpy(msg + len, "R1 is pressed, auto_shooting is ");
+        msg[strlen(msg)] = '0' + auto_shooting;
         break;
     case -10:
+        wait(0.1);
+        auto_shooting = 0;
+        pin4 = 0; 
         strcpy(msg + len, "R1 is released");
         break;
     case 11:
-        smooth = 1 - smooth;
-        strcpy(msg + len, "share is pressed, smooth: ");
-        msg[strlen(msg)] = '0' + smooth;
-        
+        strcpy(msg + len, "share is pressed");        
         break;
     case -11:
         strcpy(msg + len, "share is released");
@@ -307,8 +327,8 @@
     
     // Initiate firing pins
     firing1.mode(PullUp); // default 0
-    firing2.mode(PullUp); // default 1
-    firing3.mode(PullUp); // default 1
+    firing2.mode(PullUp); // default 0
+    firing3.mode(PullUp); // default 0
     
     // Reset dc motor
     dc_motor1.reset();
@@ -316,6 +336,8 @@
     dc_motor2.reset();
     dc_motor2.set_pid(0.008, 0, 0, 0.0);
 //    up = 1; pin2 = 1; pin2 = 0;
+//    aout.period_us(0.01); 
+    aout = 0;
     // Main programming
     int start = 0; // control variable for resetting shooter
     int tmp = 0;
@@ -331,9 +353,8 @@
         {
             myled = 0;
             if(!faul_reset && firing1 == 0)
-//            if(1)
             {
-                pin2 = 0; pin1 = 1; pin2 = 1; // 101
+                pin2 = 0; pin1 = 1; pin3 = 1; // 101
                 start = 1;
                 memset(msg, 0, sizeof(msg));
                 strcpy(msg, "Start resetting Faulhaber: firing1 is ");