Diff: main.cpp

Revision:

5:03af248f3f7c

Parent:

4:d5502eb8adcd

Child:

6:0b79b90b83a2

--- a/main.cpp	Fri Jun 11 02:19:26 2021 +0000
+++ b/main.cpp	Mon Jun 14 09:24:44 2021 +0000
@@ -5,13 +5,17 @@
 #include "Servo.h"
 #include <ros.h>
 #include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Int16MultiArray.h>
 #include "DC_Motor_Controller.h"
+//#include "DC_Motor.h"
 
 // Declare Functions
 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
 void msgCb_button(const std_msgs::Int8 &button_msg);
+void msgCam(const std_msgs::Int16 &cam_msg);
+
 void itoa(int num, char *str, int radix);
 
 // Define ros variables
@@ -19,9 +23,12 @@
 // 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("PotDistance", msgCam);
 // Pubs
 std_msgs::String str_msg;
+std_msgs::String str_msg2;
 ros::Publisher responser("display", &str_msg);
+ros::Publisher responser2("show_cam", &str_msg2);
 
 // ============  Define IOs  =============
 
@@ -30,28 +37,52 @@
 // LED
 DigitalOut myled(LED1);
 // Faulhaber pins
-DigitalOut up = A3;  // pin 1 of faulhaber
-DigitalOut down = A5; // pin 2 of faulhaber
-DigitalOut back = A4; // pin 3 of faulhaber
+DigitalOut pin1 = A3;  // pin 1 of faulhaber
+DigitalOut pin2 = A5; // pin 2 of faulhaber
+DigitalOut pin3 = A4; // pin 3 of faulhaber
+DigitalOut pin4 = A0; // pin 4 of faulhaber
+AnalogOut aout = D13;
 // Electromagnet
 DigitalOut lock = D14; 
 // Firing pin
-DigitalIn firing1 = D3; // Reset Faulhaber
+DigitalIn firing1 = D9; // Reset Faulhaber
 DigitalIn firing2 = D15; // Load 1 arrow
 DigitalIn firing3 = D2; // Pick, small
 // Valves
-DigitalOut eject_1 = A1; // valve 1
-DigitalOut eject_2 = A2;  // valve 2
+DigitalOut eject_1 = D4; // valve 1
+DigitalOut eject_2 = D7;  // valve 2
 // DC motors
-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
+DC_Motor_PID dc_motor1(D6, D5, PE_6, PF_8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
+DC_Motor_PID dc_motor2(D10, D11, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works?  original: D4, D7 for encoder, D12, D13 for out
 
 // Global Control variable
 char msg[200] = {0}; // msg for responser
+char msg2[200] = {0}; // msg for cam show
 int stage_pick = 0;  // stage of picking up arrows from the rack
 int n_loaded = 0;  // number of arrows. 
 int faul_reset = 0;
 int load_manual = 1; // tune the loading 1 arrow manually
+int auto_shooting = 0;
+int smooth = 0; 
+
+// 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)
+    {
+        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);
+    }
+}
 
 // button_msg: Int8
 void msgCb_button(const std_msgs::Int8 &button_msg)
@@ -62,7 +93,6 @@
     int len = strlen(msg);
     msg[len++] = ';';
     msg[len++] = ' ';
-    
     switch (button_msg.data)
     {
     case 1:
@@ -70,6 +100,11 @@
         eject_2 = 1 - eject_2;
         strcpy(msg + len, "x is pressed, eject_1 is ");
         msg[strlen(msg)] = '0' + int(eject_1);
+        // ---------- added for clearing n_loaded -------------
+        n_loaded = 6;         
+        strcpy(msg + strlen(msg), "n_loaded is ");
+        msg[strlen(msg)] = '0' + n_loaded;
+        // ---------- end --------------
         break;
     case -1:
         strcpy(msg + len, "x is released, eject_2 is ");
@@ -85,33 +120,37 @@
         strcpy(msg + len, "o is released");
         break;
     case 3:
-        back  = 1;
-        strcpy(msg + len, "triangle is pressed, back is ");
-        msg[strlen(msg)] = '0' + int(back);
+        pin3  = 1;
+        strcpy(msg + len, "triangle is pressed, pin3 is ");
+        msg[strlen(msg)] = '0' + int(pin3);
         break;
     case -3:
-        back = 0; 
-        strcpy(msg + len, "triangle is released, back is ");
-        msg[strlen(msg)] = '0' + int(back);
+        pin3 = 0; 
+        strcpy(msg + len, "triangle is released, pin3 is ");
+        msg[strlen(msg)] = '0' + int(pin3);
         break;
     case 4:
         strcpy(msg + len, "square is pressed");
-        up = 1; down = 1; lock = 1;
+        pin1 = 1; pin2 = 1; pin3 = 0; lock = 1;
         break;
     case -4:
         strcpy(msg + len, "square is released");
-        up = 0; down = 0;
+        pin1 = 0; pin2 = 0;
         break;
     case 5:
-        down = 1;
-        up = 0;
-        strcpy(msg + len, "down arrow is pressed, down is ");
-        msg[strlen(msg)] = '0' + int(down);
+        
+        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);
         break;
     case -5:
-        down = 0;
-        strcpy(msg + len, "down arrow is released, down is ");
-        msg[strlen(msg)] = '0' + int(down);
+        pin2 = 0; pin4 = 0;
+        strcpy(msg + len, "down arrow is released, pin2 is ");
+        msg[strlen(msg)] = '0' + int(pin2);
         break;
     case 6:
         load_manual = 0;
@@ -123,8 +162,8 @@
 
         if(n_loaded <= 5)
         {
-            dc_motor1.set_out(0,0.7);
-            wait(1.4);     
+            dc_motor1.set_out(0,0.5);
+            wait(1.7);     
             dc_motor1.set_out(0,0);           
         }
         else
@@ -132,8 +171,7 @@
             load_manual = 1;
             strcpy(msg + len, "right arrow is pressed. ");
             strcpy(msg + strlen(msg), "n_loaded cleared!");
-            n_loaded = 0;
-            
+            n_loaded = 0;            
         }
             
         break;
@@ -142,15 +180,18 @@
         strcpy(msg + len, "right arrow is released. ");
         break;
     case 7:
-        up = 1;
-        down = 0;
-        strcpy(msg + len, "up arrow is pressed, up is");
-        msg[strlen(msg)] = '0' + int(up);
+        
+        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:
-        up = 0;
-        strcpy(msg + len, "up arrow is released, up is");
-        msg[strlen(msg)] = '0' + int(up);
+        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 ");
@@ -182,7 +223,18 @@
         strcpy(msg + len, "left arrow is released, left is ");
         break;
     case 9:
-        strcpy(msg + len, "L1 is pressed");
+        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;
         break;
     case -9:
         strcpy(msg + len, "L1 is released");
@@ -196,7 +248,10 @@
         strcpy(msg + len, "R1 is released");
         break;
     case 11:
-        strcpy(msg + len, "share is pressed");
+        smooth = 1 - smooth;
+        strcpy(msg + len, "share is pressed, smooth: ");
+        msg[strlen(msg)] = '0' + smooth;
+        
         break;
     case -11:
         strcpy(msg + len, "share is released");
@@ -208,11 +263,11 @@
         strcpy(msg + len, "option is released");
         break;
     case 13:
-        up = 1; down = 1; back = 1;
+        pin1 = 1; pin2 = 1; pin3 = 1;
         strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
         break;
     case -13:
-        up = 0; down = 0; back= 0;
+        pin1 = 0; pin2 = 0; pin3= 0;
         strcpy(msg + len, "playstation button is released");
         break;
     default:
@@ -234,28 +289,33 @@
     nh.initNode();
     // Advertise publishers
     nh.advertise(responser);
+    nh.advertise(responser2);
     // Initiate display
+//    msg = "------- LAST DAY -------";
     str_msg.data = msg;
+    str_msg2.data = msg2;
 //    responser.publish(&str_msg);
+//    responser2.publish(&str_msg2);
     // Initiate subscribers
     nh.subscribe(sub_button);
     nh.subscribe(sub_joystick);
+    nh.subscribe(sub_cam);
     // Initiate IOs, keep everything at rest
     myled = 0;
-    back = 0;up = 0; down = 0; lock = 0;  
+    pin1 = 0;pin2 = 0; pin3 = 0; lock = 0;  
     eject_1 = 0; eject_2 = 1;
     
     // Initiate firing pins
-    firing1.mode(PullUp); // default 1
+    firing1.mode(PullUp); // default 0
     firing2.mode(PullUp); // default 1
     firing3.mode(PullUp); // default 1
     
     // Reset dc motor
     dc_motor1.reset();
-    dc_motor1.set_pid(0.008, 0, 0, 0.0);
+//    dc_motor1.set_pid(0.008, 0, 0, 0.0);
     dc_motor2.reset();
     dc_motor2.set_pid(0.008, 0, 0, 0.0);
-    up = 1; back = 1; down = 0;
+//    up = 1; pin2 = 1; pin2 = 0;
     // Main programming
     int start = 0; // control variable for resetting shooter
     int tmp = 0;
@@ -273,7 +333,7 @@
             if(!faul_reset && firing1 == 0)
 //            if(1)
             {
-                down = 0; up = 1; back = 1; // 101
+                pin2 = 0; pin1 = 1; pin2 = 1; // 101
                 start = 1;
                 memset(msg, 0, sizeof(msg));
                 strcpy(msg, "Start resetting Faulhaber: firing1 is ");
@@ -307,13 +367,13 @@
         }
         if(firing1.read() == 1 && start == 1)
         {
-            up = 0; down = 1; back = 1; // 110
+            pin1 = 0; pin2 = 1; pin3 = 1; // 110
             wait(0.3);
             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;
+            pin3 = 0; pin2 = 0; pin1 = 0;
             start = 0;
             
         }