Revision 11:d48dd2e111ba, committed 2021-08-16

Comitter:

howanglam3

Date:

Mon Aug 16 15:46:29 2021 +0000

Parent:

10:9720882ee8ee

Commit message:

latest

Changed in this revision

--- a/main.cpp	Thu Jun 17 10:12:21 2021 +0000
+++ b/main.cpp	Mon Aug 16 15:46:29 2021 +0000
@@ -21,7 +21,7 @@
 using namespace ros;
 using namespace geometry_msgs;
 
-DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr
+DC_Motor_PID motor(D15,D14,D4,D5,792); //out1 out2 in1 in2 ppr
 
 DigitalOut myled = LED1;
 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
@@ -30,6 +30,8 @@
 DigitalIn but(D7);              // firing pin
 DigitalOut Up = D9;
 DigitalOut down = D8; //pneumatic part
+//DigitalOut left = D14;
+//DigitalOut right = D15;
 
 LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
 
@@ -39,6 +41,7 @@
 Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0);
 
 
+
 //thread function
 void movingArmLeft()
 {
@@ -46,8 +49,10 @@
         s1.wait();
             robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // rotate down, tuned
             robot_arm.moveServos(3,1000,2,185,3,180,4,1000);
-            robot_arm.moveServos(3,500,2,700,3,180,4,1000); // rotate left, tuned
-            robot_arm.moveServos(3,1000,2,700,3,180,4,500); // open the crawler
+            robot_arm.moveServos(3,1000,2,185,3,180,4,1000);//delay
+            robot_arm.moveServos(3,750,2,630,3,180,4,1000); // rotate left, tuned
+            robot_arm.moveServos(3,1000,2,630,3,180,4,1000);//delay
+            robot_arm.moveServos(3,1000,2,630,3,180,4,500); // open the crawler
     }
 }
 void movingArmRight()
@@ -56,8 +61,10 @@
         s2.wait();
             robot_arm.moveServos(3,1000,2,185,3,540,4,1000);
             robot_arm.moveServos(3,1000,2,185,3,920,4,1000);
-            robot_arm.moveServos(3,500,2,750,3,920,4,1000); // rotate left, tuned
-            robot_arm.moveServos(3,1000,2,680,3,920,4,500);
+            robot_arm.moveServos(3,1000,2,185,3,920,4,1000);//delay
+            robot_arm.moveServos(3,750,2,680,3,920,4,1000); // rotate left, tuned
+            robot_arm.moveServos(3,1000,2,680,3,920,4,1000); //delay
+            robot_arm.moveServos(3,1000,2,590,3,920,4,500);
     }
 }
 void movingArmOpen()
@@ -85,12 +92,12 @@
         }
         motor.set_out(0,0);
         motor.reset();
-        motor.move_angle(-260);
+        motor.move_angle(-220);
     }
 }
 void moving10cm(){
     s6.wait();
-    motor.move_angle(-100);
+    motor.move_angle(-150);
 }
 void movingOrigin(){
     while(1){
@@ -102,7 +109,7 @@
         }
         motor.set_out(0,0);
         motor.reset();
-        motor.move_angle(-260);
+        motor.move_angle(-220);
     }   
 }
 //ros function
@@ -171,7 +178,7 @@
 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.moveServos(3,1500,2,520,3,540,4,1000);
+        robot_arm.moveServos(3,1500,2,520,3,920,4,1000);
         }
 }
 
@@ -182,13 +189,17 @@
     float z2 = _msg.angular.z;
     if(z1==1){
     if (x==1){                                 //  Rotate motor clockwise
-        motor.set_out(0.4, 0);
+        motor.set_out(0.2, 0);
+        //right=1;
     }
     else if(x==-1) {                          //  Rotate motor anti-clockwrise 
-        motor.set_out(0, 0.4);
+        motor.set_out(0, 0.2);
+        //left=1;
     }
     else{
         motor.set_out(0, 0);
+        //left=0;
+        //right=0;
     }
     }
     if(z2==1){