Diff: main.cpp

Revision:

6:46e625708076

Parent:

4:000065db6ae4

Child:

7:b878dce06561

--- a/main.cpp	Wed May 19 09:57:43 2021 +0000
+++ b/main.cpp	Tue Jun 01 05:47:41 2021 +0000
@@ -15,19 +15,19 @@
 using namespace ros;
 using namespace geometry_msgs;
 
-DC_Motor_Controller motor(D2,D4,D8,D7,792); //out1 out2 in1 in2 ppr
+DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr
 
 DigitalOut myled = LED1;
 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
 DigitalOut pin_A = D12;         //not in use
-DigitalOut pin_B = D11;
-DigitalOut pin_C = D10;
+DigitalIn but(D9);              // firing pin
+DigitalOut up = D11; //pneumatic part
+DigitalOut down = D10; //pneumatic part
 
-LSCServo robot_arm(D1, D0);
+LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
 
 NodeHandle nh;      //ROS node
 
-Servo servo = D6;
 
 void messagesqu(const Bool& _msg){  //callback of square
     bool check = _msg.data;
@@ -46,13 +46,6 @@
     bool check = _msg.data;
     if(check)
         robot_arm.LobotSerialActionGroupRun(2,1);
-//if(check && servo == 0) {
-//        servo = 1.0;
-//    }
-//    else if(check && servo == 1)
-//    {
-//        servo = 0; 
-//    }
 }
 
 void messagetri(const Bool& _msg){  //callback of triangle
@@ -61,30 +54,92 @@
         robot_arm.LobotSerialActionGroupRun(3,1);
 }
 
+void messageL1(const Bool& _msg){   //callback of L1  up and turn
+    bool check = _msg.data;
+    if(check){
+        up = 1;
+        wait(0.1);
+        robot_arm.LobotSerialActionGroupRun(3,1);
+        up = 0;
+        motor.move_angle(412);
+        }
+}
+
+void messageR1(const Bool& _msg){   //callback of R1, not in use for now   up and turn
+    bool check = _msg.data;
+    if(check){
+        up = 1;
+        wait(0.1);
+        robot_arm.LobotSerialActionGroupRun(3,1);
+        up = 0;
+        motor.move_angle(412);
+        }
+}
+
+void messageL3(const Bool& _msg){   //callback of R3, not in use for now   turn 10 cm
+    bool check = _msg.data;
+    if(check){
+        motor.move_angle(412);
+        }
+}
+
+void messageR3(const Bool& _msg){   //callback of R3, not in use for now   turn 10 cm
+    bool check = _msg.data;
+    if(check){
+        motor.move_angle(100);
+        }
+}
+
+void messageps(const Bool& _msg){   //callback of ps, not in use for now   turn back to 0 pulse
+    bool check = _msg.data;
+    if(check){
+        motor.set_out(0.4,0);
+        while(1)
+        {
+            if(but)
+            {
+                motor.set_out(0,0);
+                break;
+            }
+        }
+        motor.reset();
+        wait(0.5);
+        motor.move_angle(615);
+        }
+}
+
+void messagepad(const Bool& _msg){   //callback of ps, not in use for now   arm go down and release
+    bool check = _msg.data;
+    if(check){
+//        robot_arm.LobotSerialActionGroupRun(3,1);
+//        down = 1;
+//        wait(0.1);
+//        down = 0;
+        motor.goto_angle(0);
+        }
+}
+
 void messagekeypad (const Twist& _msg){     //callback of keypad
     float x = _msg.linear.x;
     float y = _msg.linear.y;
     if (x==1){                                 //  Rotate motor clockwise
-        motor.out1 = 1;
-        motor.out2 = 0;
+        motor.set_out(1, 0);
     }
     else if(x==-1) {                          //  Rotate motor anti-clockwrise 
-        motor.out1 = 0;
-        motor.out2 = 1;
+        motor.set_out(0, 1);
     }
     else{
-        motor.out1 = 0;
-        motor.out2 = 0;
+        motor.set_out(0, 0);
     }
     if (y==1){                                  //  pump air of pnumetic part
-        pin_B = 1;
+        up = 1;
     }
     else if(y==-1) {                          //  release air of pnumetic part
-        pin_C = 1;
+        down = 1;
     }
     else{
-        pin_B = 0;
-        pin_C = 0;
+        up = 0;
+        down = 0;
     }
 }
 
@@ -92,11 +147,19 @@
 Subscriber<Bool> subcro("button_cross", &messagecro);
 Subscriber<Bool> subcir("button_circle", &messagecir);
 Subscriber<Bool> subtri("button_triangle", &messagetri);
+Subscriber<Bool> subL1("button_L1", &messageL1);
+Subscriber<Bool> subR1("button_R1", &messageR1);
+Subscriber<Bool> subL3("button_L3", &messageL3);
+Subscriber<Bool> subR3("button_R3", &messageR3);
+Subscriber<Bool> subps("button_ps", &messageps);
+Subscriber<Bool> subpad("button_pad", &messagepad);
 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
 
 int main() {
     myled = 0;
-    servo = 0;
+    but.mode(PullDown);
+    
+    motor.set_pid(0.008, 0, 0, 0.10);
     
     nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
     nh.initNode();
@@ -105,7 +168,14 @@
     nh.subscribe(subcro);
     nh.subscribe(subcir);
     nh.subscribe(subtri);
+    nh.subscribe(subL1);
+    nh.subscribe(subR1);
+    nh.subscribe(subL3);
+    nh.subscribe(subR3);
     nh.subscribe(subkeypad);
+    nh.subscribe(subps);
+    nh.subscribe(subpad);
+
     while(true) {
         nh.spinOnce();
         if(nh.connected())