Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@8:e0fc4ff343f8, 2021-06-12 (annotated)

Committer:
howanglam3
Date:
Sat Jun 12 16:02:20 2021 +0000
Revision:
8:e0fc4ff343f8
Parent:
7:b878dce06561
Child:
10:9720882ee8ee
continue

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bensonsinsin998 0:858c61a9c2de 1 #include "mbed.h"
howanglam3 7:b878dce06561 2 #include "rtos/rtos.h"
howanglam3 7:b878dce06561 3 #include "Teseo-LIV3F.h"
howanglam3 7:b878dce06561 4 #include <cstdlib>
howanglam3 7:b878dce06561 5 #include <vector>
howanglam3 7:b878dce06561 6 #include "XNucleoIKS01A2.h"
howanglam3 7:b878dce06561 7
bensonsinsin998 0:858c61a9c2de 8 #include "Servo.h"
howanglam3 4:000065db6ae4 9
howanglam3 4:000065db6ae4 10 #include "LSCServo.h"
howanglam3 4:000065db6ae4 11 #include "DC_Motor_Controller.h"
bensonsinsin998 0:858c61a9c2de 12
bensonsinsin998 0:858c61a9c2de 13 #include <ros.h>
bensonsinsin998 0:858c61a9c2de 14 #include "std_msgs/Bool.h"
howanglam3 2:399bcafe6f36 15 #include "geometry_msgs/Twist.h"
bensonsinsin998 0:858c61a9c2de 16
bensonsinsin998 0:858c61a9c2de 17 #define BAUD_RATE 115200
bensonsinsin998 0:858c61a9c2de 18
bensonsinsin998 0:858c61a9c2de 19 using namespace std;
bensonsinsin998 0:858c61a9c2de 20 using namespace std_msgs;
bensonsinsin998 0:858c61a9c2de 21 using namespace ros;
howanglam3 2:399bcafe6f36 22 using namespace geometry_msgs;
bensonsinsin998 0:858c61a9c2de 23
howanglam3 6:46e625708076 24 DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr
howanglam3 2:399bcafe6f36 25
howanglam3 3:759af41b06ee 26 DigitalOut myled = LED1;
howanglam3 3:759af41b06ee 27 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
howanglam3 7:b878dce06561 28 DigitalOut pusher2 = D13;
howanglam3 7:b878dce06561 29 DigitalOut pusher1 = D12; //not in use
howanglam3 7:b878dce06561 30 DigitalIn but(D7); // firing pin
howanglam3 6:46e625708076 31 DigitalOut up = D11; //pneumatic part
howanglam3 6:46e625708076 32 DigitalOut down = D10; //pneumatic part
howanglam3 2:399bcafe6f36 33
howanglam3 6:46e625708076 34 LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
howanglam3 3:759af41b06ee 35
howanglam3 3:759af41b06ee 36 NodeHandle nh; //ROS node
bensonsinsin998 0:858c61a9c2de 37
howanglam3 7:b878dce06561 38 static Thread t1, t2, t3, t4, t5, t6, t7;
howanglam3 7:b878dce06561 39 Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0);
bensonsinsin998 0:858c61a9c2de 40
howanglam3 1:ac39b48026ca 41
howanglam3 7:b878dce06561 42 //thread function
howanglam3 7:b878dce06561 43 void movingArmLeft()
howanglam3 7:b878dce06561 44 {
howanglam3 7:b878dce06561 45 while(1){
howanglam3 7:b878dce06561 46 s1.wait();
howanglam3 7:b878dce06561 47 robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // rotate down, tuned
howanglam3 7:b878dce06561 48 robot_arm.moveServos(3,1500,2,650,3,180,4,1000); // rotate left, tuned
howanglam3 7:b878dce06561 49 Thread::wait(1000);
howanglam3 7:b878dce06561 50 robot_arm.moveServos(3,1000,2,650,3,180,4,500); // open the crawler
howanglam3 7:b878dce06561 51 }
howanglam3 6:46e625708076 52 }
howanglam3 7:b878dce06561 53 void movingArmRight()
howanglam3 7:b878dce06561 54 {
howanglam3 7:b878dce06561 55 while(1){
howanglam3 7:b878dce06561 56 s2.wait();
howanglam3 7:b878dce06561 57 robot_arm.moveServos(3,1000,2,185,3,540,4,1000);
howanglam3 7:b878dce06561 58 robot_arm.moveServos(3,1500,2,650,3,920,4,1000); // rotate left, tuned
howanglam3 7:b878dce06561 59 Thread::wait(1000);
howanglam3 7:b878dce06561 60 robot_arm.moveServos(3,1500,2,700,3,920,4,500);
howanglam3 7:b878dce06561 61 }
howanglam3 6:46e625708076 62 }
howanglam3 7:b878dce06561 63 void movingArmOpen()
howanglam3 7:b878dce06561 64 {
howanglam3 7:b878dce06561 65 while(1){
howanglam3 7:b878dce06561 66 s3.wait();
howanglam3 7:b878dce06561 67 robot_arm.moveServos(3,1000,2,185,3,540,4,500); // initial pos to put an arrow
howanglam3 7:b878dce06561 68 }
howanglam3 6:46e625708076 69 }
howanglam3 7:b878dce06561 70 void movingArmClose()
howanglam3 7:b878dce06561 71 {
howanglam3 7:b878dce06561 72 while(1){
howanglam3 7:b878dce06561 73 s4.wait();
howanglam3 7:b878dce06561 74 robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // catch/crawl the arrow
howanglam3 7:b878dce06561 75 }
howanglam3 6:46e625708076 76 }
howanglam3 7:b878dce06561 77 void moving20cm()
howanglam3 7:b878dce06561 78 {
howanglam3 7:b878dce06561 79 while(1){
howanglam3 7:b878dce06561 80 s5.wait();
howanglam3 7:b878dce06561 81 motor.set_out(0,0.4);
howanglam3 8:e0fc4ff343f8 82 while(!but)
howanglam3 6:46e625708076 83 {
howanglam3 8:e0fc4ff343f8 84 ;
howanglam3 7:b878dce06561 85 }
howanglam3 8:e0fc4ff343f8 86 motor.set_out(0,0);
howanglam3 7:b878dce06561 87 motor.reset();
howanglam3 7:b878dce06561 88 motor.move_angle(-250);
howanglam3 7:b878dce06561 89 }
howanglam3 7:b878dce06561 90 }
howanglam3 7:b878dce06561 91 void moving10cm(){
howanglam3 7:b878dce06561 92 s6.wait();
howanglam3 7:b878dce06561 93 motor.move_angle(-100);
howanglam3 7:b878dce06561 94 }
howanglam3 7:b878dce06561 95 void movingOrigin(){
howanglam3 7:b878dce06561 96 while(1){
howanglam3 7:b878dce06561 97 s7.wait();
howanglam3 7:b878dce06561 98 motor.set_out(0,1);
howanglam3 8:e0fc4ff343f8 99 while(!but)
howanglam3 7:b878dce06561 100 {
howanglam3 8:e0fc4ff343f8 101 ;
howanglam3 6:46e625708076 102 }
howanglam3 8:e0fc4ff343f8 103 motor.set_out(0,0);
howanglam3 6:46e625708076 104 motor.reset();
howanglam3 7:b878dce06561 105 motor.move_angle(-250);
howanglam3 7:b878dce06561 106 }
howanglam3 7:b878dce06561 107 }
howanglam3 7:b878dce06561 108 //ros function
howanglam3 7:b878dce06561 109 void messagesqu(const Bool& _msg){ //callback of square, open claw
howanglam3 7:b878dce06561 110 bool check = _msg.data;
howanglam3 7:b878dce06561 111 if(check)
howanglam3 7:b878dce06561 112 s3.release();
howanglam3 7:b878dce06561 113 }
howanglam3 7:b878dce06561 114
howanglam3 7:b878dce06561 115 void messagecro(const Bool& _msg){ //callback of cross, arrow point to back
howanglam3 7:b878dce06561 116 bool check = _msg.data;
howanglam3 7:b878dce06561 117 if(check)
howanglam3 7:b878dce06561 118 s2.release();
howanglam3 7:b878dce06561 119 }
howanglam3 7:b878dce06561 120
howanglam3 7:b878dce06561 121 void messagecir(const Bool& _msg){ //callback of circle, arrow point to front
howanglam3 7:b878dce06561 122 bool check = _msg.data;
howanglam3 7:b878dce06561 123 if(check)
howanglam3 7:b878dce06561 124 s1.release();
howanglam3 7:b878dce06561 125 }
howanglam3 7:b878dce06561 126
howanglam3 7:b878dce06561 127 void messagetri(const Bool& _msg){ //callback of triangle, close claw
howanglam3 7:b878dce06561 128 bool check = _msg.data;
howanglam3 7:b878dce06561 129 if(check)
howanglam3 7:b878dce06561 130 s4.release();
howanglam3 7:b878dce06561 131 }
howanglam3 7:b878dce06561 132
howanglam3 7:b878dce06561 133 void messageL1(const Bool& _msg){ //callback of L1 up pusher
howanglam3 7:b878dce06561 134 bool check = _msg.data;
howanglam3 7:b878dce06561 135 if(check){
howanglam3 7:b878dce06561 136 pusher1 = 1;
howanglam3 6:46e625708076 137 }
howanglam3 7:b878dce06561 138 else
howanglam3 7:b878dce06561 139 pusher1 = 0;
howanglam3 7:b878dce06561 140 }
howanglam3 7:b878dce06561 141
howanglam3 7:b878dce06561 142 void messageR1(const Bool& _msg){ //callback of R1, up and turn
howanglam3 7:b878dce06561 143 bool check = _msg.data;
howanglam3 7:b878dce06561 144 if(check){
howanglam3 7:b878dce06561 145 pusher2 = 1;
howanglam3 7:b878dce06561 146 }
howanglam3 7:b878dce06561 147 else
howanglam3 7:b878dce06561 148 pusher2 = 0;
howanglam3 7:b878dce06561 149 }
howanglam3 7:b878dce06561 150
howanglam3 7:b878dce06561 151 void messageL3(const Bool& _msg){ //callback of R3,turn 20 cm
howanglam3 7:b878dce06561 152 bool check = _msg.data;
howanglam3 7:b878dce06561 153 if(check){
howanglam3 7:b878dce06561 154 s5.release();
howanglam3 7:b878dce06561 155 }
howanglam3 7:b878dce06561 156 }
howanglam3 7:b878dce06561 157
howanglam3 7:b878dce06561 158 void messageR3(const Bool& _msg){ //callback of R3,turn 10 cm
howanglam3 7:b878dce06561 159 bool check = _msg.data;
howanglam3 7:b878dce06561 160 if(check){
howanglam3 7:b878dce06561 161 s6.release();
howanglam3 7:b878dce06561 162 }
howanglam3 7:b878dce06561 163 }
howanglam3 7:b878dce06561 164
howanglam3 7:b878dce06561 165 void messageps(const Bool& _msg){ //callback of ps,turn back to 0 pulse
howanglam3 7:b878dce06561 166 bool check = _msg.data;
howanglam3 7:b878dce06561 167 if(check)
howanglam3 7:b878dce06561 168 s7.release();
howanglam3 6:46e625708076 169 }
howanglam3 6:46e625708076 170
howanglam3 6:46e625708076 171 void messagepad(const Bool& _msg){ //callback of ps, not in use for now arm go down and release
howanglam3 6:46e625708076 172 bool check = _msg.data;
howanglam3 6:46e625708076 173 if(check){
howanglam3 7:b878dce06561 174 robot_arm.moveServos(3,1500,2,500,3,540,4,1000);
howanglam3 6:46e625708076 175 }
howanglam3 6:46e625708076 176 }
howanglam3 6:46e625708076 177
howanglam3 3:759af41b06ee 178 void messagekeypad (const Twist& _msg){ //callback of keypad
howanglam3 2:399bcafe6f36 179 float x = _msg.linear.x;
howanglam3 2:399bcafe6f36 180 float y = _msg.linear.y;
howanglam3 7:b878dce06561 181 if (x==-1){ // Rotate motor clockwise
howanglam3 6:46e625708076 182 motor.set_out(1, 0);
howanglam3 3:759af41b06ee 183 }
howanglam3 7:b878dce06561 184 else if(x==1) { // Rotate motor anti-clockwrise
howanglam3 6:46e625708076 185 motor.set_out(0, 1);
howanglam3 3:759af41b06ee 186 }
howanglam3 3:759af41b06ee 187 else{
howanglam3 6:46e625708076 188 motor.set_out(0, 0);
howanglam3 3:759af41b06ee 189 }
howanglam3 3:759af41b06ee 190 if (y==1){ // pump air of pnumetic part
howanglam3 6:46e625708076 191 up = 1;
howanglam3 3:759af41b06ee 192 }
howanglam3 3:759af41b06ee 193 else if(y==-1) { // release air of pnumetic part
howanglam3 6:46e625708076 194 down = 1;
howanglam3 3:759af41b06ee 195 }
howanglam3 3:759af41b06ee 196 else{
howanglam3 6:46e625708076 197 up = 0;
howanglam3 6:46e625708076 198 down = 0;
howanglam3 3:759af41b06ee 199 }
howanglam3 2:399bcafe6f36 200 }
howanglam3 2:399bcafe6f36 201
bensonsinsin998 0:858c61a9c2de 202 Subscriber<Bool> subsqu("button_square", &messagesqu);
bensonsinsin998 0:858c61a9c2de 203 Subscriber<Bool> subcro("button_cross", &messagecro);
bensonsinsin998 0:858c61a9c2de 204 Subscriber<Bool> subcir("button_circle", &messagecir);
bensonsinsin998 0:858c61a9c2de 205 Subscriber<Bool> subtri("button_triangle", &messagetri);
howanglam3 6:46e625708076 206 Subscriber<Bool> subL1("button_L1", &messageL1);
howanglam3 6:46e625708076 207 Subscriber<Bool> subR1("button_R1", &messageR1);
howanglam3 6:46e625708076 208 Subscriber<Bool> subL3("button_L3", &messageL3);
howanglam3 6:46e625708076 209 Subscriber<Bool> subR3("button_R3", &messageR3);
howanglam3 6:46e625708076 210 Subscriber<Bool> subps("button_ps", &messageps);
howanglam3 6:46e625708076 211 Subscriber<Bool> subpad("button_pad", &messagepad);
howanglam3 2:399bcafe6f36 212 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
bensonsinsin998 0:858c61a9c2de 213
bensonsinsin998 0:858c61a9c2de 214 int main() {
bensonsinsin998 0:858c61a9c2de 215 myled = 0;
howanglam3 7:b878dce06561 216 but.mode(PullUp);
howanglam3 8:e0fc4ff343f8 217 up = 0;
howanglam3 8:e0fc4ff343f8 218 down = 0;
howanglam3 8:e0fc4ff343f8 219 pusher1 = 0;
howanglam3 8:e0fc4ff343f8 220 pusher2 = 0;
howanglam3 6:46e625708076 221 motor.set_pid(0.008, 0, 0, 0.10);
bensonsinsin998 0:858c61a9c2de 222
howanglam3 3:759af41b06ee 223 nh.getHardware()->setBaud(BAUD_RATE); //set baud rate
bensonsinsin998 0:858c61a9c2de 224 nh.initNode();
bensonsinsin998 0:858c61a9c2de 225
bensonsinsin998 0:858c61a9c2de 226 nh.subscribe(subsqu);
bensonsinsin998 0:858c61a9c2de 227 nh.subscribe(subcro);
bensonsinsin998 0:858c61a9c2de 228 nh.subscribe(subcir);
bensonsinsin998 0:858c61a9c2de 229 nh.subscribe(subtri);
howanglam3 6:46e625708076 230 nh.subscribe(subL1);
howanglam3 6:46e625708076 231 nh.subscribe(subR1);
howanglam3 6:46e625708076 232 nh.subscribe(subL3);
howanglam3 6:46e625708076 233 nh.subscribe(subR3);
howanglam3 3:759af41b06ee 234 nh.subscribe(subkeypad);
howanglam3 6:46e625708076 235 nh.subscribe(subps);
howanglam3 6:46e625708076 236 nh.subscribe(subpad);
howanglam3 7:b878dce06561 237 t1.start(&movingArmLeft);
howanglam3 7:b878dce06561 238 t2.start(&movingArmRight);
howanglam3 7:b878dce06561 239 t3.start(&movingArmOpen);
howanglam3 7:b878dce06561 240 t4.start(&movingArmClose);
howanglam3 7:b878dce06561 241 t5.start(&moving20cm);
howanglam3 7:b878dce06561 242 t6.start(&moving10cm);
howanglam3 7:b878dce06561 243 t7.start(&movingOrigin);
bensonsinsin998 0:858c61a9c2de 244 while(true) {
bensonsinsin998 0:858c61a9c2de 245 nh.spinOnce();
bensonsinsin998 0:858c61a9c2de 246 if(nh.connected())
bensonsinsin998 0:858c61a9c2de 247 myled = 1;
bensonsinsin998 0:858c61a9c2de 248 else
bensonsinsin998 0:858c61a9c2de 249 myled = 0;
bensonsinsin998 0:858c61a9c2de 250 }
bensonsinsin998 0:858c61a9c2de 251 }