pin1,2,3,servo
Dependencies: Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib
main.cpp@6:46e625708076, 2021-06-01 (annotated)
- Committer:
- howanglam3
- Date:
- Tue Jun 01 05:47:41 2021 +0000
- Revision:
- 6:46e625708076
- Parent:
- 4:000065db6ae4
- Child:
- 7:b878dce06561
set up the reset of firing pin of motor(not tested)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bensonsinsin998 | 0:858c61a9c2de | 1 | #include "mbed.h" |
bensonsinsin998 | 0:858c61a9c2de | 2 | #include "Servo.h" |
howanglam3 | 4:000065db6ae4 | 3 | |
howanglam3 | 4:000065db6ae4 | 4 | #include "LSCServo.h" |
howanglam3 | 4:000065db6ae4 | 5 | #include "DC_Motor_Controller.h" |
bensonsinsin998 | 0:858c61a9c2de | 6 | |
bensonsinsin998 | 0:858c61a9c2de | 7 | #include <ros.h> |
bensonsinsin998 | 0:858c61a9c2de | 8 | #include "std_msgs/Bool.h" |
howanglam3 | 2:399bcafe6f36 | 9 | #include "geometry_msgs/Twist.h" |
bensonsinsin998 | 0:858c61a9c2de | 10 | |
bensonsinsin998 | 0:858c61a9c2de | 11 | #define BAUD_RATE 115200 |
bensonsinsin998 | 0:858c61a9c2de | 12 | |
bensonsinsin998 | 0:858c61a9c2de | 13 | using namespace std; |
bensonsinsin998 | 0:858c61a9c2de | 14 | using namespace std_msgs; |
bensonsinsin998 | 0:858c61a9c2de | 15 | using namespace ros; |
howanglam3 | 2:399bcafe6f36 | 16 | using namespace geometry_msgs; |
bensonsinsin998 | 0:858c61a9c2de | 17 | |
howanglam3 | 6:46e625708076 | 18 | DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr |
howanglam3 | 2:399bcafe6f36 | 19 | |
howanglam3 | 3:759af41b06ee | 20 | DigitalOut myled = LED1; |
howanglam3 | 3:759af41b06ee | 21 | DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4 |
howanglam3 | 3:759af41b06ee | 22 | DigitalOut pin_A = D12; //not in use |
howanglam3 | 6:46e625708076 | 23 | DigitalIn but(D9); // firing pin |
howanglam3 | 6:46e625708076 | 24 | DigitalOut up = D11; //pneumatic part |
howanglam3 | 6:46e625708076 | 25 | DigitalOut down = D10; //pneumatic part |
howanglam3 | 2:399bcafe6f36 | 26 | |
howanglam3 | 6:46e625708076 | 27 | LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx |
howanglam3 | 3:759af41b06ee | 28 | |
howanglam3 | 3:759af41b06ee | 29 | NodeHandle nh; //ROS node |
bensonsinsin998 | 0:858c61a9c2de | 30 | |
bensonsinsin998 | 0:858c61a9c2de | 31 | |
howanglam3 | 3:759af41b06ee | 32 | void messagesqu(const Bool& _msg){ //callback of square |
bensonsinsin998 | 0:858c61a9c2de | 33 | bool check = _msg.data; |
bensonsinsin998 | 0:858c61a9c2de | 34 | if(check) |
howanglam3 | 4:000065db6ae4 | 35 | robot_arm.LobotSerialActionGroupRun(0,1); |
bensonsinsin998 | 0:858c61a9c2de | 36 | } |
bensonsinsin998 | 0:858c61a9c2de | 37 | |
howanglam3 | 3:759af41b06ee | 38 | void messagecro(const Bool& _msg){ //callback of cross |
bensonsinsin998 | 0:858c61a9c2de | 39 | bool check = _msg.data; |
bensonsinsin998 | 0:858c61a9c2de | 40 | if(check) |
howanglam3 | 4:000065db6ae4 | 41 | robot_arm.LobotSerialActionGroupRun(1,1); |
bensonsinsin998 | 0:858c61a9c2de | 42 | } |
bensonsinsin998 | 0:858c61a9c2de | 43 | |
howanglam3 | 1:ac39b48026ca | 44 | |
howanglam3 | 3:759af41b06ee | 45 | void messagecir(const Bool& _msg){ //callback of circle |
bensonsinsin998 | 0:858c61a9c2de | 46 | bool check = _msg.data; |
howanglam3 | 3:759af41b06ee | 47 | if(check) |
howanglam3 | 4:000065db6ae4 | 48 | robot_arm.LobotSerialActionGroupRun(2,1); |
howanglam3 | 3:759af41b06ee | 49 | } |
howanglam3 | 3:759af41b06ee | 50 | |
howanglam3 | 3:759af41b06ee | 51 | void messagetri(const Bool& _msg){ //callback of triangle |
howanglam3 | 3:759af41b06ee | 52 | bool check = _msg.data; |
howanglam3 | 3:759af41b06ee | 53 | if(check) |
howanglam3 | 4:000065db6ae4 | 54 | robot_arm.LobotSerialActionGroupRun(3,1); |
bensonsinsin998 | 0:858c61a9c2de | 55 | } |
bensonsinsin998 | 0:858c61a9c2de | 56 | |
howanglam3 | 6:46e625708076 | 57 | void messageL1(const Bool& _msg){ //callback of L1 up and turn |
howanglam3 | 6:46e625708076 | 58 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 59 | if(check){ |
howanglam3 | 6:46e625708076 | 60 | up = 1; |
howanglam3 | 6:46e625708076 | 61 | wait(0.1); |
howanglam3 | 6:46e625708076 | 62 | robot_arm.LobotSerialActionGroupRun(3,1); |
howanglam3 | 6:46e625708076 | 63 | up = 0; |
howanglam3 | 6:46e625708076 | 64 | motor.move_angle(412); |
howanglam3 | 6:46e625708076 | 65 | } |
howanglam3 | 6:46e625708076 | 66 | } |
howanglam3 | 6:46e625708076 | 67 | |
howanglam3 | 6:46e625708076 | 68 | void messageR1(const Bool& _msg){ //callback of R1, not in use for now up and turn |
howanglam3 | 6:46e625708076 | 69 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 70 | if(check){ |
howanglam3 | 6:46e625708076 | 71 | up = 1; |
howanglam3 | 6:46e625708076 | 72 | wait(0.1); |
howanglam3 | 6:46e625708076 | 73 | robot_arm.LobotSerialActionGroupRun(3,1); |
howanglam3 | 6:46e625708076 | 74 | up = 0; |
howanglam3 | 6:46e625708076 | 75 | motor.move_angle(412); |
howanglam3 | 6:46e625708076 | 76 | } |
howanglam3 | 6:46e625708076 | 77 | } |
howanglam3 | 6:46e625708076 | 78 | |
howanglam3 | 6:46e625708076 | 79 | void messageL3(const Bool& _msg){ //callback of R3, not in use for now turn 10 cm |
howanglam3 | 6:46e625708076 | 80 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 81 | if(check){ |
howanglam3 | 6:46e625708076 | 82 | motor.move_angle(412); |
howanglam3 | 6:46e625708076 | 83 | } |
howanglam3 | 6:46e625708076 | 84 | } |
howanglam3 | 6:46e625708076 | 85 | |
howanglam3 | 6:46e625708076 | 86 | void messageR3(const Bool& _msg){ //callback of R3, not in use for now turn 10 cm |
howanglam3 | 6:46e625708076 | 87 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 88 | if(check){ |
howanglam3 | 6:46e625708076 | 89 | motor.move_angle(100); |
howanglam3 | 6:46e625708076 | 90 | } |
howanglam3 | 6:46e625708076 | 91 | } |
howanglam3 | 6:46e625708076 | 92 | |
howanglam3 | 6:46e625708076 | 93 | void messageps(const Bool& _msg){ //callback of ps, not in use for now turn back to 0 pulse |
howanglam3 | 6:46e625708076 | 94 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 95 | if(check){ |
howanglam3 | 6:46e625708076 | 96 | motor.set_out(0.4,0); |
howanglam3 | 6:46e625708076 | 97 | while(1) |
howanglam3 | 6:46e625708076 | 98 | { |
howanglam3 | 6:46e625708076 | 99 | if(but) |
howanglam3 | 6:46e625708076 | 100 | { |
howanglam3 | 6:46e625708076 | 101 | motor.set_out(0,0); |
howanglam3 | 6:46e625708076 | 102 | break; |
howanglam3 | 6:46e625708076 | 103 | } |
howanglam3 | 6:46e625708076 | 104 | } |
howanglam3 | 6:46e625708076 | 105 | motor.reset(); |
howanglam3 | 6:46e625708076 | 106 | wait(0.5); |
howanglam3 | 6:46e625708076 | 107 | motor.move_angle(615); |
howanglam3 | 6:46e625708076 | 108 | } |
howanglam3 | 6:46e625708076 | 109 | } |
howanglam3 | 6:46e625708076 | 110 | |
howanglam3 | 6:46e625708076 | 111 | void messagepad(const Bool& _msg){ //callback of ps, not in use for now arm go down and release |
howanglam3 | 6:46e625708076 | 112 | bool check = _msg.data; |
howanglam3 | 6:46e625708076 | 113 | if(check){ |
howanglam3 | 6:46e625708076 | 114 | // robot_arm.LobotSerialActionGroupRun(3,1); |
howanglam3 | 6:46e625708076 | 115 | // down = 1; |
howanglam3 | 6:46e625708076 | 116 | // wait(0.1); |
howanglam3 | 6:46e625708076 | 117 | // down = 0; |
howanglam3 | 6:46e625708076 | 118 | motor.goto_angle(0); |
howanglam3 | 6:46e625708076 | 119 | } |
howanglam3 | 6:46e625708076 | 120 | } |
howanglam3 | 6:46e625708076 | 121 | |
howanglam3 | 3:759af41b06ee | 122 | void messagekeypad (const Twist& _msg){ //callback of keypad |
howanglam3 | 2:399bcafe6f36 | 123 | float x = _msg.linear.x; |
howanglam3 | 2:399bcafe6f36 | 124 | float y = _msg.linear.y; |
howanglam3 | 3:759af41b06ee | 125 | if (x==1){ // Rotate motor clockwise |
howanglam3 | 6:46e625708076 | 126 | motor.set_out(1, 0); |
howanglam3 | 3:759af41b06ee | 127 | } |
howanglam3 | 3:759af41b06ee | 128 | else if(x==-1) { // Rotate motor anti-clockwrise |
howanglam3 | 6:46e625708076 | 129 | motor.set_out(0, 1); |
howanglam3 | 3:759af41b06ee | 130 | } |
howanglam3 | 3:759af41b06ee | 131 | else{ |
howanglam3 | 6:46e625708076 | 132 | motor.set_out(0, 0); |
howanglam3 | 3:759af41b06ee | 133 | } |
howanglam3 | 3:759af41b06ee | 134 | if (y==1){ // pump air of pnumetic part |
howanglam3 | 6:46e625708076 | 135 | up = 1; |
howanglam3 | 3:759af41b06ee | 136 | } |
howanglam3 | 3:759af41b06ee | 137 | else if(y==-1) { // release air of pnumetic part |
howanglam3 | 6:46e625708076 | 138 | down = 1; |
howanglam3 | 3:759af41b06ee | 139 | } |
howanglam3 | 3:759af41b06ee | 140 | else{ |
howanglam3 | 6:46e625708076 | 141 | up = 0; |
howanglam3 | 6:46e625708076 | 142 | down = 0; |
howanglam3 | 3:759af41b06ee | 143 | } |
howanglam3 | 2:399bcafe6f36 | 144 | } |
howanglam3 | 2:399bcafe6f36 | 145 | |
bensonsinsin998 | 0:858c61a9c2de | 146 | Subscriber<Bool> subsqu("button_square", &messagesqu); |
bensonsinsin998 | 0:858c61a9c2de | 147 | Subscriber<Bool> subcro("button_cross", &messagecro); |
bensonsinsin998 | 0:858c61a9c2de | 148 | Subscriber<Bool> subcir("button_circle", &messagecir); |
bensonsinsin998 | 0:858c61a9c2de | 149 | Subscriber<Bool> subtri("button_triangle", &messagetri); |
howanglam3 | 6:46e625708076 | 150 | Subscriber<Bool> subL1("button_L1", &messageL1); |
howanglam3 | 6:46e625708076 | 151 | Subscriber<Bool> subR1("button_R1", &messageR1); |
howanglam3 | 6:46e625708076 | 152 | Subscriber<Bool> subL3("button_L3", &messageL3); |
howanglam3 | 6:46e625708076 | 153 | Subscriber<Bool> subR3("button_R3", &messageR3); |
howanglam3 | 6:46e625708076 | 154 | Subscriber<Bool> subps("button_ps", &messageps); |
howanglam3 | 6:46e625708076 | 155 | Subscriber<Bool> subpad("button_pad", &messagepad); |
howanglam3 | 2:399bcafe6f36 | 156 | Subscriber<Twist> subkeypad("button_keypad", &messagekeypad); |
bensonsinsin998 | 0:858c61a9c2de | 157 | |
bensonsinsin998 | 0:858c61a9c2de | 158 | int main() { |
bensonsinsin998 | 0:858c61a9c2de | 159 | myled = 0; |
howanglam3 | 6:46e625708076 | 160 | but.mode(PullDown); |
howanglam3 | 6:46e625708076 | 161 | |
howanglam3 | 6:46e625708076 | 162 | motor.set_pid(0.008, 0, 0, 0.10); |
bensonsinsin998 | 0:858c61a9c2de | 163 | |
howanglam3 | 3:759af41b06ee | 164 | nh.getHardware()->setBaud(BAUD_RATE); //set baud rate |
bensonsinsin998 | 0:858c61a9c2de | 165 | nh.initNode(); |
bensonsinsin998 | 0:858c61a9c2de | 166 | |
bensonsinsin998 | 0:858c61a9c2de | 167 | nh.subscribe(subsqu); |
bensonsinsin998 | 0:858c61a9c2de | 168 | nh.subscribe(subcro); |
bensonsinsin998 | 0:858c61a9c2de | 169 | nh.subscribe(subcir); |
bensonsinsin998 | 0:858c61a9c2de | 170 | nh.subscribe(subtri); |
howanglam3 | 6:46e625708076 | 171 | nh.subscribe(subL1); |
howanglam3 | 6:46e625708076 | 172 | nh.subscribe(subR1); |
howanglam3 | 6:46e625708076 | 173 | nh.subscribe(subL3); |
howanglam3 | 6:46e625708076 | 174 | nh.subscribe(subR3); |
howanglam3 | 3:759af41b06ee | 175 | nh.subscribe(subkeypad); |
howanglam3 | 6:46e625708076 | 176 | nh.subscribe(subps); |
howanglam3 | 6:46e625708076 | 177 | nh.subscribe(subpad); |
howanglam3 | 6:46e625708076 | 178 | |
bensonsinsin998 | 0:858c61a9c2de | 179 | while(true) { |
bensonsinsin998 | 0:858c61a9c2de | 180 | nh.spinOnce(); |
bensonsinsin998 | 0:858c61a9c2de | 181 | if(nh.connected()) |
bensonsinsin998 | 0:858c61a9c2de | 182 | myled = 1; |
bensonsinsin998 | 0:858c61a9c2de | 183 | else |
bensonsinsin998 | 0:858c61a9c2de | 184 | myled = 0; |
bensonsinsin998 | 0:858c61a9c2de | 185 | } |
bensonsinsin998 | 0:858c61a9c2de | 186 | } |