Diff: main.cpp

Revision:

7:b878dce06561

Parent:

6:46e625708076

Child:

8:e0fc4ff343f8

--- a/main.cpp	Tue Jun 01 05:47:41 2021 +0000
+++ b/main.cpp	Sat Jun 12 09:23:05 2021 +0000
@@ -1,4 +1,10 @@
 #include "mbed.h"
+#include "rtos/rtos.h"
+#include "Teseo-LIV3F.h"
+#include <cstdlib>
+#include <vector>
+#include "XNucleoIKS01A2.h"
+
 #include "Servo.h"
 
 #include "LSCServo.h"
@@ -19,8 +25,9 @@
 
 DigitalOut myled = LED1;
 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
-DigitalOut pin_A = D12;         //not in use
-DigitalIn but(D9);              // firing pin
+DigitalOut pusher2 = D13;
+DigitalOut pusher1 = D12;         //not in use
+DigitalIn but(D7);              // firing pin
 DigitalOut up = D11; //pneumatic part
 DigitalOut down = D10; //pneumatic part
 
@@ -28,104 +35,157 @@
 
 NodeHandle nh;      //ROS node
 
-
-void messagesqu(const Bool& _msg){  //callback of square
-    bool check = _msg.data;
-    if(check)
-        robot_arm.LobotSerialActionGroupRun(0,1);
-}
-
-void messagecro(const Bool& _msg){  //callback of cross
-    bool check = _msg.data;
-    if(check)
-        robot_arm.LobotSerialActionGroupRun(1,1);
-}
+static Thread t1, t2, t3, t4, t5, t6, t7;
+Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0);
 
 
-void messagecir(const Bool& _msg){  //callback of circle
-    bool check = _msg.data;
-    if(check)
-        robot_arm.LobotSerialActionGroupRun(2,1);
-}
-
-void messagetri(const Bool& _msg){  //callback of triangle
-    bool check = _msg.data;
-    if(check)
-        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);
-        }
+//thread function
+void movingArmLeft()
+{
+    while(1){
+        s1.wait();
+            robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // rotate down, tuned
+            robot_arm.moveServos(3,1500,2,650,3,180,4,1000); // rotate left, tuned
+            Thread::wait(1000);
+            robot_arm.moveServos(3,1000,2,650,3,180,4,500); // open the crawler
+    }
 }
-
-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 movingArmRight()
+{
+    while(1){
+        s2.wait();
+            robot_arm.moveServos(3,1000,2,185,3,540,4,1000);
+            robot_arm.moveServos(3,1500,2,650,3,920,4,1000); // rotate left, tuned
+            Thread::wait(1000);
+            robot_arm.moveServos(3,1500,2,700,3,920,4,500);
+    }
 }
-
-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 movingArmOpen()
+{
+    while(1){
+        s3.wait();
+            robot_arm.moveServos(3,1000,2,185,3,540,4,500);      // initial pos to put an arrow
+    }
 }
-
-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 movingArmClose()
+{
+    while(1){
+        s4.wait();
+            robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // catch/crawl the arrow
+    }
 }
-
-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);
+void moving20cm()
+{
+    while(1){
+        s5.wait();
+        motor.set_out(0,0.4);
         while(1)
         {
-            if(but)
-            {
+           if(but){
+                motor.set_out(0,0);
+                break;
+           }
+        }
+        motor.reset();
+        motor.move_angle(-250);
+    }
+}
+void moving10cm(){
+    s6.wait();
+    motor.move_angle(-100);
+}
+void movingOrigin(){
+    while(1){
+        s7.wait();
+        motor.set_out(0,1);
+        while(1)
+        {
+            if(but) {
                 motor.set_out(0,0);
                 break;
             }
         }
         motor.reset();
-        wait(0.5);
-        motor.move_angle(615);
+        motor.move_angle(-250);
+    }   
+}
+//ros function
+void messagesqu(const Bool& _msg){  //callback of square, open claw 
+    bool check = _msg.data;
+    if(check)
+        s3.release();
+}
+
+void messagecro(const Bool& _msg){  //callback of cross, arrow point to back
+    bool check = _msg.data;
+    if(check)
+        s2.release();
+}
+
+void messagecir(const Bool& _msg){  //callback of circle, arrow point to front
+    bool check = _msg.data;
+    if(check)
+        s1.release();
+}
+
+void messagetri(const Bool& _msg){  //callback of triangle, close claw
+    bool check = _msg.data;
+    if(check)
+        s4.release();
+}
+
+void messageL1(const Bool& _msg){   //callback of L1  up pusher
+    bool check = _msg.data;
+    if(check){
+        pusher1 = 1;
         }
+    else
+        pusher1 = 0;
+}
+
+void messageR1(const Bool& _msg){   //callback of R1, up and turn
+    bool check = _msg.data;
+    if(check){
+        pusher2 = 1;
+        }
+    else
+        pusher2 = 0;
+}
+
+void messageL3(const Bool& _msg){   //callback of R3,turn 20 cm
+    bool check = _msg.data;
+    if(check){
+        s5.release();
+        }
+}
+
+void messageR3(const Bool& _msg){   //callback of R3,turn 10 cm
+    bool check = _msg.data;
+    if(check){
+        s6.release();
+        }
+}
+
+void messageps(const Bool& _msg){   //callback of ps,turn back to 0 pulse
+    bool check = _msg.data;
+    if(check)
+        s7.release();
 }
 
 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);
+        robot_arm.moveServos(3,1500,2,500,3,540,4,1000);
         }
 }
 
 void messagekeypad (const Twist& _msg){     //callback of keypad
     float x = _msg.linear.x;
     float y = _msg.linear.y;
-    if (x==1){                                 //  Rotate motor clockwise
+    if (x==-1){                                 //  Rotate motor clockwise
         motor.set_out(1, 0);
     }
-    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
+    else if(x==1) {                          //  Rotate motor anti-clockwrise 
         motor.set_out(0, 1);
     }
     else{
@@ -157,7 +217,7 @@
 
 int main() {
     myled = 0;
-    but.mode(PullDown);
+    but.mode(PullUp);
     
     motor.set_pid(0.008, 0, 0, 0.10);
     
@@ -175,7 +235,13 @@
     nh.subscribe(subkeypad);
     nh.subscribe(subps);
     nh.subscribe(subpad);
-
+    t1.start(&movingArmLeft);
+    t2.start(&movingArmRight);
+    t3.start(&movingArmOpen);
+    t4.start(&movingArmClose);
+    t5.start(&moving20cm);
+    t6.start(&moving10cm);
+    t7.start(&movingOrigin);
     while(true) {
         nh.spinOnce();
         if(nh.connected())