Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@11:d48dd2e111ba, 2021-08-16 (annotated)

Committer:
howanglam3
Date:
Mon Aug 16 15:46:29 2021 +0000
Revision:
11:d48dd2e111ba
Parent:
10:9720882ee8ee 
latest

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 11:d48dd2e111ba 24 DC_Motor_PID motor(D15,D14,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 10:9720882ee8ee 31 DigitalOut Up = D9;
howanglam3 10:9720882ee8ee 32 DigitalOut down = D8; //pneumatic part
howanglam3 11:d48dd2e111ba 33 //DigitalOut left = D14;
howanglam3 11:d48dd2e111ba 34 //DigitalOut right = D15;
howanglam3 2:399bcafe6f36 35
howanglam3 6:46e625708076 36 LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx
howanglam3 3:759af41b06ee 37
howanglam3 3:759af41b06ee 38 NodeHandle nh; //ROS node
bensonsinsin998 0:858c61a9c2de 39
howanglam3 7:b878dce06561 40 static Thread t1, t2, t3, t4, t5, t6, t7;
howanglam3 7:b878dce06561 41 Semaphore s1(0), s2(0), s3(0), s4(0), s5(0), s6(0), s7(0);
bensonsinsin998 0:858c61a9c2de 42
howanglam3 1:ac39b48026ca 43
howanglam3 11:d48dd2e111ba 44
howanglam3 7:b878dce06561 45 //thread function
howanglam3 7:b878dce06561 46 void movingArmLeft()
howanglam3 7:b878dce06561 47 {
howanglam3 7:b878dce06561 48 while(1){
howanglam3 7:b878dce06561 49 s1.wait();
howanglam3 7:b878dce06561 50 robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // rotate down, tuned
howanglam3 10:9720882ee8ee 51 robot_arm.moveServos(3,1000,2,185,3,180,4,1000);
howanglam3 11:d48dd2e111ba 52 robot_arm.moveServos(3,1000,2,185,3,180,4,1000);//delay
howanglam3 11:d48dd2e111ba 53 robot_arm.moveServos(3,750,2,630,3,180,4,1000); // rotate left, tuned
howanglam3 11:d48dd2e111ba 54 robot_arm.moveServos(3,1000,2,630,3,180,4,1000);//delay
howanglam3 11:d48dd2e111ba 55 robot_arm.moveServos(3,1000,2,630,3,180,4,500); // open the crawler
howanglam3 7:b878dce06561 56 }
howanglam3 6:46e625708076 57 }
howanglam3 7:b878dce06561 58 void movingArmRight()
howanglam3 7:b878dce06561 59 {
howanglam3 7:b878dce06561 60 while(1){
howanglam3 7:b878dce06561 61 s2.wait();
howanglam3 7:b878dce06561 62 robot_arm.moveServos(3,1000,2,185,3,540,4,1000);
howanglam3 10:9720882ee8ee 63 robot_arm.moveServos(3,1000,2,185,3,920,4,1000);
howanglam3 11:d48dd2e111ba 64 robot_arm.moveServos(3,1000,2,185,3,920,4,1000);//delay
howanglam3 11:d48dd2e111ba 65 robot_arm.moveServos(3,750,2,680,3,920,4,1000); // rotate left, tuned
howanglam3 11:d48dd2e111ba 66 robot_arm.moveServos(3,1000,2,680,3,920,4,1000); //delay
howanglam3 11:d48dd2e111ba 67 robot_arm.moveServos(3,1000,2,590,3,920,4,500);
howanglam3 7:b878dce06561 68 }
howanglam3 6:46e625708076 69 }
howanglam3 7:b878dce06561 70 void movingArmOpen()
howanglam3 7:b878dce06561 71 {
howanglam3 7:b878dce06561 72 while(1){
howanglam3 7:b878dce06561 73 s3.wait();
howanglam3 7:b878dce06561 74 robot_arm.moveServos(3,1000,2,185,3,540,4,500); // initial pos to put an arrow
howanglam3 7:b878dce06561 75 }
howanglam3 6:46e625708076 76 }
howanglam3 7:b878dce06561 77 void movingArmClose()
howanglam3 7:b878dce06561 78 {
howanglam3 7:b878dce06561 79 while(1){
howanglam3 7:b878dce06561 80 s4.wait();
howanglam3 7:b878dce06561 81 robot_arm.moveServos(3,1000,2,185,3,540,4,1000); // catch/crawl the arrow
howanglam3 7:b878dce06561 82 }
howanglam3 6:46e625708076 83 }
howanglam3 7:b878dce06561 84 void moving20cm()
howanglam3 7:b878dce06561 85 {
howanglam3 7:b878dce06561 86 while(1){
howanglam3 7:b878dce06561 87 s5.wait();
howanglam3 7:b878dce06561 88 motor.set_out(0,0.4);
howanglam3 8:e0fc4ff343f8 89 while(!but)
howanglam3 6:46e625708076 90 {
howanglam3 10:9720882ee8ee 91 motor.set_out(0,0.4);
howanglam3 7:b878dce06561 92 }
howanglam3 8:e0fc4ff343f8 93 motor.set_out(0,0);
howanglam3 7:b878dce06561 94 motor.reset();
howanglam3 11:d48dd2e111ba 95 motor.move_angle(-220);
howanglam3 7:b878dce06561 96 }
howanglam3 7:b878dce06561 97 }
howanglam3 7:b878dce06561 98 void moving10cm(){
howanglam3 7:b878dce06561 99 s6.wait();
howanglam3 11:d48dd2e111ba 100 motor.move_angle(-150);
howanglam3 7:b878dce06561 101 }
howanglam3 7:b878dce06561 102 void movingOrigin(){
howanglam3 7:b878dce06561 103 while(1){
howanglam3 7:b878dce06561 104 s7.wait();
howanglam3 7:b878dce06561 105 motor.set_out(0,1);
howanglam3 8:e0fc4ff343f8 106 while(!but)
howanglam3 7:b878dce06561 107 {
howanglam3 10:9720882ee8ee 108 motor.set_out(0, 1);
howanglam3 6:46e625708076 109 }
howanglam3 8:e0fc4ff343f8 110 motor.set_out(0,0);
howanglam3 6:46e625708076 111 motor.reset();
howanglam3 11:d48dd2e111ba 112 motor.move_angle(-220);
howanglam3 7:b878dce06561 113 }
howanglam3 7:b878dce06561 114 }
howanglam3 7:b878dce06561 115 //ros function
howanglam3 7:b878dce06561 116 void messagesqu(const Bool& _msg){ //callback of square, open claw
howanglam3 7:b878dce06561 117 bool check = _msg.data;
howanglam3 7:b878dce06561 118 if(check)
howanglam3 7:b878dce06561 119 s3.release();
howanglam3 7:b878dce06561 120 }
howanglam3 7:b878dce06561 121
howanglam3 7:b878dce06561 122 void messagecro(const Bool& _msg){ //callback of cross, arrow point to back
howanglam3 7:b878dce06561 123 bool check = _msg.data;
howanglam3 7:b878dce06561 124 if(check)
howanglam3 7:b878dce06561 125 s2.release();
howanglam3 7:b878dce06561 126 }
howanglam3 7:b878dce06561 127
howanglam3 7:b878dce06561 128 void messagecir(const Bool& _msg){ //callback of circle, arrow point to front
howanglam3 7:b878dce06561 129 bool check = _msg.data;
howanglam3 7:b878dce06561 130 if(check)
howanglam3 7:b878dce06561 131 s1.release();
howanglam3 7:b878dce06561 132 }
howanglam3 7:b878dce06561 133
howanglam3 7:b878dce06561 134 void messagetri(const Bool& _msg){ //callback of triangle, close claw
howanglam3 7:b878dce06561 135 bool check = _msg.data;
howanglam3 7:b878dce06561 136 if(check)
howanglam3 7:b878dce06561 137 s4.release();
howanglam3 7:b878dce06561 138 }
howanglam3 7:b878dce06561 139
howanglam3 7:b878dce06561 140 void messageL1(const Bool& _msg){ //callback of L1 up pusher
howanglam3 7:b878dce06561 141 bool check = _msg.data;
howanglam3 7:b878dce06561 142 if(check){
howanglam3 7:b878dce06561 143 pusher1 = 1;
howanglam3 6:46e625708076 144 }
howanglam3 7:b878dce06561 145 else
howanglam3 7:b878dce06561 146 pusher1 = 0;
howanglam3 7:b878dce06561 147 }
howanglam3 7:b878dce06561 148
howanglam3 7:b878dce06561 149 void messageR1(const Bool& _msg){ //callback of R1, up and turn
howanglam3 7:b878dce06561 150 bool check = _msg.data;
howanglam3 7:b878dce06561 151 if(check){
howanglam3 7:b878dce06561 152 pusher2 = 1;
howanglam3 7:b878dce06561 153 }
howanglam3 7:b878dce06561 154 else
howanglam3 7:b878dce06561 155 pusher2 = 0;
howanglam3 7:b878dce06561 156 }
howanglam3 7:b878dce06561 157
howanglam3 7:b878dce06561 158 void messageL3(const Bool& _msg){ //callback of R3,turn 20 cm
howanglam3 7:b878dce06561 159 bool check = _msg.data;
howanglam3 7:b878dce06561 160 if(check){
howanglam3 7:b878dce06561 161 s5.release();
howanglam3 7:b878dce06561 162 }
howanglam3 7:b878dce06561 163 }
howanglam3 7:b878dce06561 164
howanglam3 7:b878dce06561 165 void messageR3(const Bool& _msg){ //callback of R3,turn 10 cm
howanglam3 7:b878dce06561 166 bool check = _msg.data;
howanglam3 7:b878dce06561 167 if(check){
howanglam3 7:b878dce06561 168 s6.release();
howanglam3 7:b878dce06561 169 }
howanglam3 7:b878dce06561 170 }
howanglam3 7:b878dce06561 171
howanglam3 7:b878dce06561 172 void messageps(const Bool& _msg){ //callback of ps,turn back to 0 pulse
howanglam3 7:b878dce06561 173 bool check = _msg.data;
howanglam3 7:b878dce06561 174 if(check)
howanglam3 7:b878dce06561 175 s7.release();
howanglam3 6:46e625708076 176 }
howanglam3 6:46e625708076 177
howanglam3 6:46e625708076 178 void messagepad(const Bool& _msg){ //callback of ps, not in use for now arm go down and release
howanglam3 6:46e625708076 179 bool check = _msg.data;
howanglam3 6:46e625708076 180 if(check){
howanglam3 11:d48dd2e111ba 181 robot_arm.moveServos(3,1500,2,520,3,920,4,1000);
howanglam3 6:46e625708076 182 }
howanglam3 6:46e625708076 183 }
howanglam3 6:46e625708076 184
howanglam3 3:759af41b06ee 185 void messagekeypad (const Twist& _msg){ //callback of keypad
howanglam3 2:399bcafe6f36 186 float x = _msg.linear.x;
howanglam3 2:399bcafe6f36 187 float y = _msg.linear.y;
howanglam3 10:9720882ee8ee 188 float z1 = _msg.linear.z;
howanglam3 10:9720882ee8ee 189 float z2 = _msg.angular.z;
howanglam3 10:9720882ee8ee 190 if(z1==1){
howanglam3 10:9720882ee8ee 191 if (x==1){ // Rotate motor clockwise
howanglam3 11:d48dd2e111ba 192 motor.set_out(0.2, 0);
howanglam3 11:d48dd2e111ba 193 //right=1;
howanglam3 3:759af41b06ee 194 }
howanglam3 10:9720882ee8ee 195 else if(x==-1) { // Rotate motor anti-clockwrise
howanglam3 11:d48dd2e111ba 196 motor.set_out(0, 0.2);
howanglam3 11:d48dd2e111ba 197 //left=1;
howanglam3 3:759af41b06ee 198 }
howanglam3 3:759af41b06ee 199 else{
howanglam3 6:46e625708076 200 motor.set_out(0, 0);
howanglam3 11:d48dd2e111ba 201 //left=0;
howanglam3 11:d48dd2e111ba 202 //right=0;
howanglam3 3:759af41b06ee 203 }
howanglam3 3:759af41b06ee 204 }
howanglam3 10:9720882ee8ee 205 if(z2==1){
howanglam3 10:9720882ee8ee 206 if(y ==-1.0)
howanglam3 10:9720882ee8ee 207 {
howanglam3 10:9720882ee8ee 208 Up = 1;
howanglam3 10:9720882ee8ee 209 }
howanglam3 10:9720882ee8ee 210 else if(y==1) { // release air of pnumetic part
howanglam3 6:46e625708076 211 down = 1;
howanglam3 3:759af41b06ee 212 }
howanglam3 3:759af41b06ee 213 else{
howanglam3 10:9720882ee8ee 214 Up = 0;
howanglam3 6:46e625708076 215 down = 0;
howanglam3 3:759af41b06ee 216 }
howanglam3 10:9720882ee8ee 217 }
howanglam3 2:399bcafe6f36 218 }
howanglam3 2:399bcafe6f36 219
bensonsinsin998 0:858c61a9c2de 220 Subscriber<Bool> subsqu("button_square", &messagesqu);
bensonsinsin998 0:858c61a9c2de 221 Subscriber<Bool> subcro("button_cross", &messagecro);
bensonsinsin998 0:858c61a9c2de 222 Subscriber<Bool> subcir("button_circle", &messagecir);
bensonsinsin998 0:858c61a9c2de 223 Subscriber<Bool> subtri("button_triangle", &messagetri);
howanglam3 6:46e625708076 224 Subscriber<Bool> subL1("button_L1", &messageL1);
howanglam3 6:46e625708076 225 Subscriber<Bool> subR1("button_R1", &messageR1);
howanglam3 6:46e625708076 226 Subscriber<Bool> subL3("button_L3", &messageL3);
howanglam3 6:46e625708076 227 Subscriber<Bool> subR3("button_R3", &messageR3);
howanglam3 6:46e625708076 228 Subscriber<Bool> subps("button_ps", &messageps);
howanglam3 6:46e625708076 229 Subscriber<Bool> subpad("button_pad", &messagepad);
howanglam3 2:399bcafe6f36 230 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
bensonsinsin998 0:858c61a9c2de 231
bensonsinsin998 0:858c61a9c2de 232 int main() {
bensonsinsin998 0:858c61a9c2de 233 myled = 0;
howanglam3 7:b878dce06561 234 but.mode(PullUp);
howanglam3 10:9720882ee8ee 235 Up = 0;
howanglam3 8:e0fc4ff343f8 236 down = 0;
howanglam3 8:e0fc4ff343f8 237 pusher1 = 0;
howanglam3 8:e0fc4ff343f8 238 pusher2 = 0;
howanglam3 6:46e625708076 239 motor.set_pid(0.008, 0, 0, 0.10);
howanglam3 10:9720882ee8ee 240
howanglam3 3:759af41b06ee 241 nh.getHardware()->setBaud(BAUD_RATE); //set baud rate
bensonsinsin998 0:858c61a9c2de 242 nh.initNode();
bensonsinsin998 0:858c61a9c2de 243
bensonsinsin998 0:858c61a9c2de 244 nh.subscribe(subsqu);
bensonsinsin998 0:858c61a9c2de 245 nh.subscribe(subcro);
bensonsinsin998 0:858c61a9c2de 246 nh.subscribe(subcir);
bensonsinsin998 0:858c61a9c2de 247 nh.subscribe(subtri);
howanglam3 6:46e625708076 248 nh.subscribe(subL1);
howanglam3 6:46e625708076 249 nh.subscribe(subR1);
howanglam3 6:46e625708076 250 nh.subscribe(subL3);
howanglam3 6:46e625708076 251 nh.subscribe(subR3);
howanglam3 3:759af41b06ee 252 nh.subscribe(subkeypad);
howanglam3 6:46e625708076 253 nh.subscribe(subps);
howanglam3 6:46e625708076 254 nh.subscribe(subpad);
howanglam3 7:b878dce06561 255 t1.start(&movingArmLeft);
howanglam3 7:b878dce06561 256 t2.start(&movingArmRight);
howanglam3 7:b878dce06561 257 t3.start(&movingArmOpen);
howanglam3 7:b878dce06561 258 t4.start(&movingArmClose);
howanglam3 7:b878dce06561 259 t5.start(&moving20cm);
howanglam3 7:b878dce06561 260 t6.start(&moving10cm);
howanglam3 7:b878dce06561 261 t7.start(&movingOrigin);
bensonsinsin998 0:858c61a9c2de 262 while(true) {
bensonsinsin998 0:858c61a9c2de 263 nh.spinOnce();
bensonsinsin998 0:858c61a9c2de 264 if(nh.connected())
bensonsinsin998 0:858c61a9c2de 265 myled = 1;
bensonsinsin998 0:858c61a9c2de 266 else
bensonsinsin998 0:858c61a9c2de 267 myled = 0;
bensonsinsin998 0:858c61a9c2de 268 }
bensonsinsin998 0:858c61a9c2de 269 }