pin1,2,3,servo
Dependencies: Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib
main.cpp
- Committer:
- howanglam3
- Date:
- 2021-08-16
- Revision:
- 11:d48dd2e111ba
- Parent:
- 10:9720882ee8ee
File content as of revision 11:d48dd2e111ba:
#include "mbed.h" #include "rtos/rtos.h" #include "Teseo-LIV3F.h" #include <cstdlib> #include <vector> #include "XNucleoIKS01A2.h" #include "Servo.h" #include "LSCServo.h" #include "DC_Motor_Controller.h" #include <ros.h> #include "std_msgs/Bool.h" #include "geometry_msgs/Twist.h" #define BAUD_RATE 115200 using namespace std; using namespace std_msgs; using namespace ros; using namespace geometry_msgs; 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 DigitalOut pusher2 = D13; DigitalOut pusher1 = D12; //not in use 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 NodeHandle nh; //ROS node static Thread t1, t2, t3, t4, t5, t6, t7; Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0); //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,1000,2,185,3,180,4,1000); 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() { while(1){ 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,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() { while(1){ s3.wait(); robot_arm.moveServos(3,1000,2,185,3,540,4,500); // initial pos to put an arrow } } void movingArmClose() { while(1){ s4.wait(); robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // catch/crawl the arrow } } void moving20cm() { while(1){ s5.wait(); motor.set_out(0,0.4); while(!but) { motor.set_out(0,0.4); } motor.set_out(0,0); motor.reset(); motor.move_angle(-220); } } void moving10cm(){ s6.wait(); motor.move_angle(-150); } void movingOrigin(){ while(1){ s7.wait(); motor.set_out(0,1); while(!but) { motor.set_out(0, 1); } motor.set_out(0,0); motor.reset(); motor.move_angle(-220); } } //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.moveServos(3,1500,2,520,3,920,4,1000); } } void messagekeypad (const Twist& _msg){ //callback of keypad float x = _msg.linear.x; float y = _msg.linear.y; float z1 = _msg.linear.z; float z2 = _msg.angular.z; if(z1==1){ if (x==1){ // Rotate motor clockwise motor.set_out(0.2, 0); //right=1; } else if(x==-1) { // Rotate motor anti-clockwrise motor.set_out(0, 0.2); //left=1; } else{ motor.set_out(0, 0); //left=0; //right=0; } } if(z2==1){ if(y ==-1.0) { Up = 1; } else if(y==1) { // release air of pnumetic part down = 1; } else{ Up = 0; down = 0; } } } Subscriber<Bool> subsqu("button_square", &messagesqu); 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; but.mode(PullUp); 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(); nh.subscribe(subsqu); 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); 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()) myled = 1; else myled = 0; } }