pin1,2,3,servo
Dependencies: Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 11:d48dd2e111ba
- Parent:
- 10:9720882ee8ee
diff -r 9720882ee8ee -r d48dd2e111ba main.cpp --- 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){