Robocon 2021 / ros_button_2021

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?

UserRevisionLine numberNew 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 }