Revision 8:9abe58bd7c07, committed 2021-06-18

Comitter:

ftppr

Date:

Fri Jun 18 16:34:51 2021 +0000

Parent:

6:0b79b90b83a2

Commit message:

;

Changed in this revision

--- a/DC_Motor.lib	Wed Jun 16 10:10:57 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/Robocon-2021/code/DC_Motor/#c04bc378d73f

--- a/main.cpp	Wed Jun 16 10:10:57 2021 +0000
+++ b/main.cpp	Fri Jun 18 16:34:51 2021 +0000
@@ -18,6 +18,26 @@
 
 void itoa(int num, char *str, int radix);
 
+// ROS Connect Baud Rate 
+#define BAUD_RATE 115200
+
+#include "mbed.h"
+#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"
+
+
+// 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
 ros::NodeHandle nh;
 // Subs
@@ -37,24 +57,30 @@
 DigitalIn button(USER_BUTTON);
 // LED
 DigitalOut myled(LED1);
-// Faulhaber pins
-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
+// Faulhaber1 pins
+DigitalOut pin1 = A3;  // pin 1 of faulhaber1
+DigitalOut pin2 = A5; // pin 2 of faulhaber1
+DigitalOut pin3 = A4; // pin 3 of faulhaber1
+DigitalOut pin4 = A0; // pin 4 of faulhaber1
 AnalogOut aout = D13;
+// Faulhaber2 pins
+DigitalOut faul2_1 = A1; // pin1 of faulhaber2
+DigitalOut faul2_2 = A2; // pin2 of faulhaber2
 // Electromagnet
 DigitalOut lock = D14; 
 // Firing pin
-DigitalIn firing1 = D9; // Reset Faulhaber
+DigitalIn firing1 = D2; // Reset Faulhaber
 DigitalIn firing2 = D15; // Load 1 arrow
-DigitalIn firing3 = D2; // Pick, small
+DigitalIn firing3 = D7; // Pick, short
+DigitalIn firing4 = D4; // Pick, longError: Identifier "faul2_1" is undefined in "main.cpp", Line: 241, Col: 14
+DigitalIn firing5 = D8; // Pick, long - 2
+DigitalIn firing6 = D9;
 // Valves
-DigitalOut eject_1 = D4; // valve 1
-DigitalOut eject_2 = D7;  // valve 2
+DigitalOut valve = D3; // valve 
+
 // DC motors
 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
+DC_Motor_PID dc_motor2(D10, D12, 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
@@ -65,48 +91,49 @@
 int load_manual = 1; // tune the loading 1 arrow manually
 int auto_shooting = 0;
 int smooth = 0; 
-
+int auto_shooting_goToTargetPos = 0;
+int start = 0; // control variable for resetting shooter
+int touch = 0;
 // cam callback
 void msgCam(const std_msgs::Int16 &cam_msg)
 {
-    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)
     {
-        int outValue[3] = [cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10];
-        if(auto_shooting)
+        
+        int outValue[3] = {cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10};
+        if(auto_shooting && auto_shooting_goToTargetPos)
         {
-            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);
+            pin1 = 0;
+            pin2 = 0;
+            pin3 = 1;
+            wait(0.2);
+            aout = 1;
+            pin3 = 1;
+            pin2 = 1;
+            pin1 = 1;
+            wait(0.1);
+            auto_shooting_goToTargetPos = false;
+            
             for(int i = 0; i<3; i++){
-                outValue[i]/8;
-                (outValue[i]/4)%2;
-                (outValue[i]/2
-                (outValue[i]%2;
+                //int val[4] = {outValue[i]/8, outValue[i]/4%2, outValue[i]/2%2, outValue[i]%2};
+                aout = outValue[i]/8;
+                pin3 = outValue[i]/4%2;
+                pin2= outValue[i]/2%2;
+                pin1 = outValue[i]%2;
+                wait(0.1);
+                aout = 1;
+                pin3 = 1;
+                pin2 = 1;
+                pin1 = 1;
+                wait(0.1);
             }
+            aout = 0; pin3 = 0; pin2 = 0; pin1 = 0; pin4 = 0;
         }
+        memset(msg2, 0, sizeof(msg2));
+        strcpy(msg2, "Cam distance: ");
+        itoa(cam_msg.data, msg2 + strlen(msg2), 10);
+        responser2.publish(&str_msg2);
     }
-    
 }
 
 // button_msg: Int8
@@ -121,19 +148,13 @@
     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);
-        // ---------- added for clearing n_loaded -------------
-        n_loaded = 6;         
-        strcpy(msg + strlen(msg), "n_loaded is ");
-        msg[strlen(msg)] = '0' + n_loaded;
-        // ---------- end --------------
+        valve = 1 - valve;
+        strcpy(msg + len, "x is pressed, valve is ");
+        msg[strlen(msg)] = '0' + int(valve);
         break;
     case -1:
-        strcpy(msg + len, "x is released, eject_2 is ");
-        msg[strlen(msg)] = '0' + int(eject_2);
+//        strcpy(msg + len, "x is released, eject_2 is ");
+//        msg[strlen(msg)] = '0' + int(eject_2);
         break;
     case 2:
         lock = 1 - lock;
@@ -146,6 +167,7 @@
         break;
     case 3:
         pin3  = 1;
+        auto_shooting_goToTargetPos = true;
         strcpy(msg + len, "triangle is pressed, pin3 is ");
         msg[strlen(msg)] = '0' + int(pin3);
         break;
@@ -176,27 +198,29 @@
         msg[strlen(msg)] = '0' + int(pin2);
         break;
     case 6:
-        load_manual = 0;
-        strcpy(msg + len, "right arrow is pressed. ");
+       strcpy(msg + len, "right arrow is pressed. ");
         msg[strlen(msg)] = '0' + n_loaded;
         dc_motor1.set_out(0,1);    
         n_loaded++;  
-        while(firing2 == 0);
+        while(firing2 == 0)
+        {
+            wait(0.05);
+        }
 
         if(n_loaded <= 5)
         {
-            dc_motor1.set_out(0,0.5);
-            wait(1.7);     
+            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. ");
+            wait(0.21);
+            dc_motor1.set_out(0,0);  
             strcpy(msg + strlen(msg), "n_loaded cleared!");
-            n_loaded = 0;            
+            n_loaded = 0;
+            
         }
-            
         break;
     case -6:        
         strcpy(msg + len, "right arrow is released. ");
@@ -215,41 +239,91 @@
     case 8:
         strcpy(msg + len, "left arrow is pressed, phase is ");
         msg[strlen(msg)] = '0' + stage_pick;
+        strcpy(msg + strlen(msg), " ");
+        
         if(stage_pick == 0)
         {
-            dc_motor2.set_out(0, 1);
-//            if(firing3 == 1)
-//            {
-//                dc_motor2.set_out(0,0);
-//                strcpy(msg + strlen(msg), ", firing pin detected. ");
-//            }
+            strcpy(msg + strlen(msg), "dc_motor reset  ");
+            dc_motor2.set_out(0.5, 0);
+            while(firing3 == 0);
+            if(firing3 == 1)
+            {
+                dc_motor2.set_out(0,0);
+                strcpy(msg + strlen(msg), "short: firing3 pin detected  ");
+            }
             stage_pick++;
-            eject_1 = 1; eject_2 = 0;
-            strcpy(msg + strlen(msg), "rotate out");
+//            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++;
-            strcpy(msg + strlen(msg), "rotate phase 1");
+            strcpy(msg + strlen(msg), "faulhaber 2 RefPos ");
+            faul2_1 = 1;
+            while(firing4 == 0);
+            faul2_1 = 0;  
+            strcpy(msg + strlen(msg), "long: firing4 detected "); //內
+            stage_pick++;      
+            
         }
         else if(stage_pick == 2)
         {
-            dc_motor2.set_out(0.5,0);
-            wait(0.3);
+            strcpy(msg + strlen(msg), "dc_motor rotate to pick up ");
+            dc_motor2.set_out(0,0.5);
+            wait(0.2);
             dc_motor2.set_out(0,0);
+            stage_pick++;
+//            stage_pick = 0;
+            
+        }
+        else if(stage_pick == 3)
+        {
+            strcpy(msg + strlen(msg), "faul2 to pick the arrows "); // 外
+            faul2_2 = 1;
+            while(firing5 == 0);
+            wait(0.3);
+            faul2_2 = 0;
+            strcpy(msg + strlen(msg), "Firing5 touched "); 
+            
+            stage_pick++; // use firing 5
+//            stage_pick = 0;
+        }
+        else if(stage_pick == 4)
+        {                
+            strcpy(msg + strlen(msg), "faul2 rotate to vertical \n\r"); // 內一啲
+            faul2_1 = 1;
+            wait(2.6);
+            faul2_1 = 0;
+            stage_pick++;
+//            stage_pick = 0;
+        }
+        else if(stage_pick == 5)
+        {
+            strcpy(msg + strlen(msg), "dc_motor rotate to final pos\n\r");
+            dc_motor2.set_out(0,1);
+//            wait(2); // when there is load; otherwise 0.3
+            while(firing6 == 0);
+//            {
+//                wait(0.025);
+//            }
+            dc_motor2.set_out(0,0);
+            stage_pick++;
+//            stage_pick = 0;
+        }
+        else if(stage_pick == 6)
+        {
+            strcpy(msg + strlen(msg), "faul2 rotate to final pos \n\r"); // 內一啲
+            faul2_1 = 1;
+            wait(1.9);
+            faul2_1 = 0;
             stage_pick = 0;
-            strcpy(msg + strlen(msg), "rotate phase 2");
         }
+        
         break;
     case -8:
-//        strcpy(msg + len, "left arrow is released, left is ");
+    
         break;
     case 9:
         strcpy(msg + len, "L1 is pressed");
-//        msg[strlen(msg)] = '0' + auto_shooting;
+        
         break;
     case -9:
 //        pin3 = 0; pin4 = 0;
@@ -261,6 +335,7 @@
         dc_motor2.set_out(0,0);
         auto_shooting = 1;
         pin4 = 1; 
+        n_loaded = 6;
         strcpy(msg + len, "R1 is pressed, auto_shooting is ");
         msg[strlen(msg)] = '0' + auto_shooting;
         break;
@@ -271,7 +346,24 @@
         strcpy(msg + len, "R1 is released");
         break;
     case 11:
-        strcpy(msg + len, "share is pressed");        
+        strcpy(msg + len, "share is pressed");  
+        strcpy(msg + strlen(msg), "Start resetting Faulhaber: firing1 is ");
+        msg[strlen(msg)] = '0' + firing1.read();
+        responser.publish(&str_msg);
+        if(!faul_reset && firing1 == 0)
+        {
+            pin2 = 0; pin1 = 1; pin3 = 1; // 101
+            memset(msg, 0, sizeof(msg));
+            
+//                faul_reset = 1;
+        }
+         while(firing1 == 0);
+        pin1 = 0; pin2 = 1; pin3 = 1; // 110
+        wait(0.3);
+        pin1 = 0; pin2 = 0; pin3 = 0;
+//        memset(msg, 0, sizeof(msg));
+        strcpy(msg + strlen(msg), " case 11 firing1 is touched");
+              
         break;
     case -11:
         strcpy(msg + len, "share is released");
@@ -323,13 +415,15 @@
     // Initiate IOs, keep everything at rest
     myled = 0;
     pin1 = 0;pin2 = 0; pin3 = 0; lock = 0;  
-    eject_1 = 0; eject_2 = 1;
+    valve = 0; faul2_1 = 0; faul2_2 = 0;
     
     // Initiate firing pins
     firing1.mode(PullUp); // default 0
     firing2.mode(PullUp); // default 0
     firing3.mode(PullUp); // default 0
-    
+    firing4.mode(PullUp); // default 0
+    firing5.mode(PullUp); // default 0
+    firing6.mode(PullUp); // default 0
     // Reset dc motor
     dc_motor1.reset();
 //    dc_motor1.set_pid(0.008, 0, 0, 0.0);
@@ -339,9 +433,7 @@
 //    aout.period_us(0.01); 
     aout = 0;
     // Main programming
-    int start = 0; // control variable for resetting shooter
-    int tmp = 0;
-
+    
     while(true) {
         nh.spinOnce();
         
@@ -349,55 +441,8 @@
             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 && firing1 == 0)
-            {
-                pin2 = 0; pin1 = 1; pin3 = 1; // 101
-                start = 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
-            {
-                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;           
-            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)
-        {
-            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() == 1 && start == 1)
-        {
-            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);
-            pin3 = 0; pin2 = 0; pin1 = 0;
-            start = 0;
-            
-        }
+
+    
     }
 }
 
@@ -502,7 +547,7 @@
     // -----------R2------------
     if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
     {
-        strcpy(msg + len, "L3: the value is ");
+        strcpy(msg + len, "R2: the value is ");
         len = strlen(msg);
         itoa(joystick_msg.data[2], msg + len, 10);