Diff: main.cpp

Revision:

2:e570143bf35e

Parent:

1:fbdbf4ac12c6

Child:

3:2441bcef5885

--- a/main.cpp	Wed Jun 02 13:53:04 2021 +0000
+++ b/main.cpp	Wed Jun 09 02:39:27 2021 +0000
@@ -1,4 +1,3 @@
-    // Hello World to sweep a servo through its full range
 // ROS Connect Baud Rate 
 #define BAUD_RATE 115200
 
@@ -8,33 +7,310 @@
 #include <std_msgs/Int8.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Int16MultiArray.h>
-ros::NodeHandle nh;
-DigitalOut myled(LED1);
+#include "DC_Motor_Controller.h"
+
+// Declare Functions
 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
 void msgCb_button(const std_msgs::Int8 &button_msg);
+void itoa(int num, char *str, int radix);
 
+// Define ros variables
+ros::NodeHandle nh;
+// Subs
 ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
 ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
-
+// Pubs
 std_msgs::String str_msg;
 ros::Publisher responser("display", &str_msg);
 
-char msg[200] = {0};
+// ============  Define IOs  =============
 
+// External Button
 DigitalIn button(USER_BUTTON);
-
-DigitalOut up = D12; // pin 1 of faulhaber
+// LED
+DigitalOut myled(LED1);
+// Faulhaber pins
+DigitalOut up = A3;  // pin 1 of faulhaber
 DigitalOut down = A5; // pin 2 of faulhaber
-DigitalOut left = D2; // DC motor 1 (via L298N control pin 1)
-DigitalOut right = D4; // DC motor 2 (via L298N control pin 2)
-//AnalogOut left = A0; // DC motor 1 (via L298N control pin 1)
-//AnalogOut right = A1; // DC motor 2 (via L298N control pin 2)
-DigitalOut lock = D14; // 電磁石
 DigitalOut back = A4; // pin 3 of faulhaber
-DigitalOut eject_1 = D10; // valve 1
-DigitalOut eject_2 = D5; // valve 2
-//DigitalOut savePos = D0; savepos by back=1 & up=1 & down=1
-DigitalIn firing = D3;
+// Electromagnet
+DigitalOut lock = D14; 
+// Firing pin
+DigitalIn firing1 = D3; // 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
+// 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
+
+// Global Control variable
+char msg[200] = {0}; // msg for responser
+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
+
+// button_msg: Int8
+void msgCb_button(const std_msgs::Int8 &button_msg)
+{
+    memset(msg, 0, sizeof(msg));
+    strcpy(msg, "Received ");
+    itoa(button_msg.data, msg + 9, 10);
+    int len = strlen(msg);
+    msg[len++] = ';';
+    msg[len++] = ' ';
+    
+    switch (button_msg.data)
+    {
+    case 1:
+        eject_1 = 1 - eject_1;
+        eject_2 = 1 - eject_2;
+        strcpy(msg + len, "x is pressed, eject_1 is ");
+        msg[strlen(msg)] = '0' + int(eject_1);
+        break;
+    case -1:
+        strcpy(msg + len, "x is released, eject_2 is ");
+        msg[strlen(msg)] = '0' + int(eject_2);
+        break;
+    case 2:
+        lock = 1 - lock;
+        strcpy(msg + len, "o is pressed, lock is: ");
+        msg[strlen(msg)] = '0' + int(lock);
+        
+        break;
+    case -2:
+        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);
+        break;
+    case -3:
+        back = 0; 
+        strcpy(msg + len, "triangle is released, back is ");
+        msg[strlen(msg)] = '0' + int(back);
+        break;
+    case 4:
+        strcpy(msg + len, "square is pressed");
+        up = 1; down = 1;
+        break;
+    case -4:
+        strcpy(msg + len, "square is released");
+        up = 0; down = 0;
+        break;
+    case 5:
+        down = 1;
+        up = 0;
+        strcpy(msg + len, "down arrow is pressed, down is ");
+        msg[strlen(msg)] = '0' + int(down);
+        break;
+    case -5:
+        down = 0;
+        strcpy(msg + len, "down arrow is released, down is ");
+        msg[strlen(msg)] = '0' + int(down);
+        break;
+    case 6:
+        load_manual = 0;
+        strcpy(msg + len, "right arrow is pressed. ");
+        msg[strlen(msg)] = '0' + n_loaded;
+        dc_motor1.set_out(0,1);    
+        n_loaded++;  
+        if(firing2 == 1)
+        {
+            if(n_loaded <= 5)
+            {
+                dc_motor1.set_out(0,0.5);
+                wait(1.2);                
+            }
+            else
+            {
+                strcpy(msg + len, "right arrow is pressed. ");
+                strcpy(msg + strlen(msg), "n_loaded cleared!");
+                n_loaded = 0;
+            }
+            dc_motor1.set_out(0,0);
+        }
+//        load_manual = 1;
+        break;
+    case -6:
+        
+        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);
+        break;
+    case -7:
+        up = 0;
+        strcpy(msg + len, "up arrow is released, up is");
+        msg[strlen(msg)] = '0' + int(up);
+        break;
+    case 8:
+        strcpy(msg + len, "left arrow is pressed, left 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);
+            stage_pick++;
+            eject_1 = 1; eject_2 = 0;
+        }
+        else if(stage_pick == 1)
+        {
+            dc_motor2.set_out(0.5,0);
+            wait(0.3);
+            dc_motor2.set_out(0,0);
+            stage_pick++;
+        }
+        else if(stage_pick == 2)
+        {
+            dc_motor2.set_out(0.5,0);
+            wait(0.3);
+            dc_motor2.set_out(0,0);
+            stage_pick = 0;
+        }
+        break;
+    case -8:
+        strcpy(msg + len, "left arrow is released, left is ");
+        break;
+    case 9:
+        strcpy(msg + len, "L1 is pressed");
+        break;
+    case -9:
+        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");
+        break;
+    case -10:
+        strcpy(msg + len, "R1 is released");
+        break;
+    case 11:
+        strcpy(msg + len, "share is pressed");
+        break;
+    case -11:
+        strcpy(msg + len, "share is released");
+        break;
+    case 12:
+        strcpy(msg + len, "option is pressed");
+        break;
+    case -12:
+        strcpy(msg + len, "option is released");
+        break;
+    case 13:
+        up = 1; down = 1; back = 1;
+        strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
+        break;
+    case -13:
+        up = 0; down = 0; back= 0;
+        strcpy(msg + len, "playstation button is released");
+        break;
+    default:
+        len = strlen(msg);
+        strcpy(msg + len, "Unrecognized pattern");
+        break;
+    }
+
+    responser.publish(&str_msg);
+}
+
+
+// --------------- main -----------------
+int main() 
+{  
+    
+    // -> Set Baud Rate
+    nh.getHardware()->setBaud(BAUD_RATE);
+    nh.initNode();
+    // Advertise publishers
+    nh.advertise(responser);
+    // Initiate display
+    str_msg.data = msg;
+//    responser.publish(&str_msg);
+    // Initiate subscribers
+    nh.subscribe(sub_button);
+    nh.subscribe(sub_joystick);
+    // Initiate IOs, keep everything at rest
+    myled = 0;
+    back = 0;up = 0; down = 0; lock = 0;  
+    eject_1 = 0; eject_2 = 1;
+    
+    // Initiate firing pins
+    firing1.mode(PullUp); // default 1
+    firing2.mode(PullDown); //default 0
+    firing3.mode(PullUp); // default 1
+    
+    // Reset dc motor
+    dc_motor1.reset();
+    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);
+//    wait(0.3);
+//    dc_motor2.move_angle(100);
+    dc_motor2.set_out(0,0);
+//     dc_motor1.move_angle(100);
+//    dc_motor1.set_out(0.5,0); // -> testing only
+    
+    // Main programming
+    int start = 0; // control variable for resetting shooter
+    int tmp = 0;
+
+    while(true) {
+        nh.spinOnce();
+        
+        if(nh.connected())
+            myled = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
+        else 
+            myled = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
+        if(button == 1)
+        {
+            myled = 0;
+            if(!faul_reset)
+            {
+                down = 0; up = 1; back = 1; // 101
+                start = 1;
+                faul_reset = 1;
+            }
+            else
+            {
+                dc_motor1.set_out(0,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;            
+            responser.publish(&str_msg);
+        }
+        if(load_manual && firing2 == 1)
+        {
+            dc_motor1.set_out(0,0);   
+        }
+        if(firing1.read() == 0 && 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);
+            
+        }
+    }
+}
 
  
 void itoa(int num, char *str, int radix)
@@ -140,188 +416,8 @@
         strcpy(msg + len, "L3: the value is ");
         len = strlen(msg);
         itoa(joystick_msg.data[2], msg + len, 10);
+        
     }
 
     responser.publish(&str_msg);
-}
-
-// button_msg: Int8
-void msgCb_button(const std_msgs::Int8 &button_msg)
-{
-//    myled = !myled; // blink the led
-    memset(msg, 0, sizeof(msg));
-    strcpy(msg, "Received ");
-    itoa(button_msg.data, msg + 9, 10);
-    //    int len = my_strlen(msg);
-    int len = strlen(msg);
-    msg[len++] = ';';
-    msg[len++] = ' ';
-    
-    switch (button_msg.data)
-    {
-    case 1:
-        eject_1 = 1 - eject_1;
-        eject_2 = 1 - eject_2;
-        strcpy(msg + len, "x is pressed, eject_1 is ");
-        msg[strlen(msg)] = '0' + int(eject_1);
-        break;
-    case -1:
-//        eject = 0;
-        strcpy(msg + len, "x is released, eject_2 is ");
-        msg[strlen(msg)] = '0' + int(eject_2);
-        break;
-    case 2:
-        lock = 1 - lock;
-        strcpy(msg + len, "o is pressed, lock is: ");
-        msg[strlen(msg)] = '0' + int(lock);
-        
-        break;
-    case -2:
-        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);
-        break;
-    case -3:
-        back = 0; 
-        strcpy(msg + len, "triangle is released, back is ");
-        msg[strlen(msg)] = '0' + int(back);
-        break;
-    case 4:
-        strcpy(msg + len, "square is pressed");
-        up = 1; down = 1;
-        break;
-    case -4:
-        strcpy(msg + len, "square is released");
-        up = 0; down = 0;
-        break;
-    case 5:
-        down = 1;
-        up = 0;
-        strcpy(msg + len, "down arrow is pressed, down is ");
-        msg[strlen(msg)] = '0' + int(down);
-        break;
-    case -5:
-        down = 0;
-        strcpy(msg + len, "down arrow is released, down is ");
-        msg[strlen(msg)] = '0' + int(down);
-        break;
-    case 6:
-        left = 0; right = 1;
-        strcpy(msg + len, "right arrow is pressed, right is ");
-        msg[strlen(msg)] = '0' + int(right);
-        break;
-    case -6:
-        right = 0;
-        strcpy(msg + len, "right arrow is released, right is ");
-        msg[strlen(msg)] = '0' + int(right);
-        break;
-    case 7:
-        up = 1;
-        down = 0;
-        strcpy(msg + len, "up arrow is pressed, up is");
-        msg[strlen(msg)] = '0' + int(up);
-        break;
-    case -7:
-        up = 0;
-        strcpy(msg + len, "up arrow is released, up is");
-        msg[strlen(msg)] = '0' + int(up);
-        break;
-    case 8:
-        left = 1; right = 0;
-        strcpy(msg + len, "left arrow is pressed, left is ");
-        msg[strlen(msg)] = '0' + int(left);
-        break;
-    case -8:
-        left = 0;
-        strcpy(msg + len, "left arrow is released, left is ");
-        msg[strlen(msg)] = '0' + int(left);
-        break;
-    case 9:
-        strcpy(msg + len, "L1 is pressed, left is ");
-        msg[strlen(msg)] = '0' + int(left);
-        break;
-    case -9:
-        strcpy(msg + len, "L1 is released");
-        break;
-    case 10:
-        strcpy(msg + len, "R1 is pressed");
-        break;
-    case -10:
-        strcpy(msg + len, "R1 is released");
-        break;
-    case 11:
-        strcpy(msg + len, "share is pressed");
-        break;
-    case -11:
-        strcpy(msg + len, "share is released");
-        break;
-    case 12:
-        strcpy(msg + len, "option is pressed");
-        break;
-    case -12:
-        strcpy(msg + len, "option is released");
-        break;
-    case 13:
-        up = 1; down = 1; back = 1;
-        strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
-        break;
-    case -13:
-        up = 0; down = 0; back= 0;
-        strcpy(msg + len, "playstation button is released");
-        break;
-    default:
-        len = strlen(msg);
-        strcpy(msg + len, "Unrecognized pattern");
-        break;
-    }
-
-    responser.publish(&str_msg);
-}
-int main() 
-{  
-    // -> Set Baud Rate
-    nh.getHardware()->setBaud(BAUD_RATE);
-    nh.initNode();
-    nh.advertise(responser);
-    str_msg.data = msg;
-    responser.publish(&str_msg);
-    nh.subscribe(sub_button);
-    nh.subscribe(sub_joystick);
-    myled = 0;
-    back = 0;up = 0; down = 0; lock = 0; left = 0; right = 0;
-    eject_1 = 0; eject_2 = 1;
-    strcpy(msg, "Byebye world.");
-    responser.publish(&str_msg);
-    int start = 1;
-    firing.mode(PullDown);
-    // Main programming
-    while(true) {
-        nh.spinOnce();
-
-        if(nh.connected())
-            myled = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
-        else 
-            myled = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
-        if(button == 1)
-        {
-            myled = 0;
-            down = 0; up = 1; back = 1;
-            start = 1;
-            while(button == 1);
-        }
-        if(firing.read() == 1 && start == 1)
-        {
-            up = 0;
-            down = 1; 
-            back = 1;
-            while(firing.read() == 1);
-            wait(0.3);
-            down = 0; back = 0; up = 0;
-            start = 0;
-            
-        }
-    }
 }
\ No newline at end of file