Diff: main.cpp

Revision:

10:9720882ee8ee

Parent:

8:e0fc4ff343f8

Child:

11:d48dd2e111ba

--- a/main.cpp	Sat Jun 12 16:06:09 2021 +0000
+++ b/main.cpp	Thu Jun 17 10:12:21 2021 +0000
@@ -28,8 +28,8 @@
 DigitalOut pusher2 = D13;
 DigitalOut pusher1 = D12;         //not in use
 DigitalIn but(D7);              // firing pin
-DigitalOut up = D11; //pneumatic part
-DigitalOut down = D10; //pneumatic part
+DigitalOut Up = D9;
+DigitalOut down = D8; //pneumatic part
 
 LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
 
@@ -45,9 +45,9 @@
     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
+            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
     }
 }
 void movingArmRight()
@@ -55,9 +55,9 @@
     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);
+            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);
     }
 }
 void movingArmOpen()
@@ -81,11 +81,11 @@
         motor.set_out(0,0.4);
         while(!but)
         {
-            ;
+        motor.set_out(0,0.4);
         }
         motor.set_out(0,0);
         motor.reset();
-        motor.move_angle(-250);
+        motor.move_angle(-260);
     }
 }
 void moving10cm(){
@@ -98,11 +98,11 @@
         motor.set_out(0,1);
         while(!but)
         {
-            ;
+            motor.set_out(0, 1);
         }
         motor.set_out(0,0);
         motor.reset();
-        motor.move_angle(-250);
+        motor.move_angle(-260);
     }   
 }
 //ros function
@@ -171,32 +171,39 @@
 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,500,3,540,4,1000);
+        robot_arm.moveServos(3,1500,2,520,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
-        motor.set_out(1, 0);
+    float z1 = _msg.linear.z;
+    float z2 = _msg.angular.z;
+    if(z1==1){
+    if (x==1){                                 //  Rotate motor clockwise
+        motor.set_out(0.4, 0);
     }
-    else if(x==1) {                          //  Rotate motor anti-clockwrise 
-        motor.set_out(0, 1);
+    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
+        motor.set_out(0, 0.4);
     }
     else{
         motor.set_out(0, 0);
     }
-    if (y==1){                                  //  pump air of pnumetic part
-        up = 1;
     }
-    else if(y==-1) {                          //  release air of pnumetic part
+    if(z2==1){
+    if(y ==-1.0)
+    {
+        Up = 1;
+    }
+    else if(y==1) {                          //  release air of pnumetic part
         down = 1;
     }
     else{
-        up = 0;
+        Up = 0;
         down = 0;
     }
+    }
 }
 
 Subscriber<Bool> subsqu("button_square", &messagesqu);
@@ -214,12 +221,12 @@
 int main() {
     myled = 0;
     but.mode(PullUp);
-    up = 0;
+    Up = 0;
     down = 0;
     pusher1 = 0;
     pusher2 = 0;
     motor.set_pid(0.008, 0, 0, 0.10);
-    
+
     nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
     nh.initNode();