Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@4:000065db6ae4, 2021-05-19 (annotated)

Committer:
howanglam3
Date:
Wed May 19 09:52:01 2021 +0000
Revision:
4:000065db6ae4
Parent:
3:759af41b06ee
Child:
6:46e625708076
Set the motor and robot arm function into header form. (not checked)

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 4:000065db6ae4 18 DC_Motor_Controller motor(D2,D4,D8,D7,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 3:759af41b06ee 23 DigitalOut pin_B = D11;
howanglam3 3:759af41b06ee 24 DigitalOut pin_C = D10;
howanglam3 2:399bcafe6f36 25
howanglam3 4:000065db6ae4 26 LSCServo robot_arm(D1, D0);
howanglam3 3:759af41b06ee 27
howanglam3 3:759af41b06ee 28 NodeHandle nh; //ROS node
bensonsinsin998 0:858c61a9c2de 29
bensonsinsin998 0:858c61a9c2de 30 Servo servo = D6;
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 //if(check && servo == 0) {
howanglam3 3:759af41b06ee 50 // servo = 1.0;
howanglam3 3:759af41b06ee 51 // }
howanglam3 3:759af41b06ee 52 // else if(check && servo == 1)
howanglam3 3:759af41b06ee 53 // {
howanglam3 3:759af41b06ee 54 // servo = 0;
howanglam3 3:759af41b06ee 55 // }
howanglam3 3:759af41b06ee 56 }
howanglam3 3:759af41b06ee 57
howanglam3 3:759af41b06ee 58 void messagetri(const Bool& _msg){ //callback of triangle
howanglam3 3:759af41b06ee 59 bool check = _msg.data;
howanglam3 3:759af41b06ee 60 if(check)
howanglam3 4:000065db6ae4 61 robot_arm.LobotSerialActionGroupRun(3,1);
bensonsinsin998 0:858c61a9c2de 62 }
bensonsinsin998 0:858c61a9c2de 63
howanglam3 3:759af41b06ee 64 void messagekeypad (const Twist& _msg){ //callback of keypad
howanglam3 2:399bcafe6f36 65 float x = _msg.linear.x;
howanglam3 2:399bcafe6f36 66 float y = _msg.linear.y;
howanglam3 3:759af41b06ee 67 if (x==1){ // Rotate motor clockwise
howanglam3 4:000065db6ae4 68 motor.out1 = 1;
howanglam3 4:000065db6ae4 69 motor.out2 = 0;
howanglam3 3:759af41b06ee 70 }
howanglam3 3:759af41b06ee 71 else if(x==-1) { // Rotate motor anti-clockwrise
howanglam3 4:000065db6ae4 72 motor.out1 = 0;
howanglam3 4:000065db6ae4 73 motor.out2 = 1;
howanglam3 3:759af41b06ee 74 }
howanglam3 3:759af41b06ee 75 else{
howanglam3 4:000065db6ae4 76 motor.out1 = 0;
howanglam3 4:000065db6ae4 77 motor.out2 = 0;
howanglam3 3:759af41b06ee 78 }
howanglam3 3:759af41b06ee 79 if (y==1){ // pump air of pnumetic part
howanglam3 3:759af41b06ee 80 pin_B = 1;
howanglam3 3:759af41b06ee 81 }
howanglam3 3:759af41b06ee 82 else if(y==-1) { // release air of pnumetic part
howanglam3 3:759af41b06ee 83 pin_C = 1;
howanglam3 3:759af41b06ee 84 }
howanglam3 3:759af41b06ee 85 else{
howanglam3 3:759af41b06ee 86 pin_B = 0;
howanglam3 3:759af41b06ee 87 pin_C = 0;
howanglam3 3:759af41b06ee 88 }
howanglam3 2:399bcafe6f36 89 }
howanglam3 2:399bcafe6f36 90
bensonsinsin998 0:858c61a9c2de 91 Subscriber<Bool> subsqu("button_square", &messagesqu);
bensonsinsin998 0:858c61a9c2de 92 Subscriber<Bool> subcro("button_cross", &messagecro);
bensonsinsin998 0:858c61a9c2de 93 Subscriber<Bool> subcir("button_circle", &messagecir);
bensonsinsin998 0:858c61a9c2de 94 Subscriber<Bool> subtri("button_triangle", &messagetri);
howanglam3 2:399bcafe6f36 95 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
bensonsinsin998 0:858c61a9c2de 96
bensonsinsin998 0:858c61a9c2de 97 int main() {
bensonsinsin998 0:858c61a9c2de 98 myled = 0;
bensonsinsin998 0:858c61a9c2de 99 servo = 0;
bensonsinsin998 0:858c61a9c2de 100
howanglam3 3:759af41b06ee 101 nh.getHardware()->setBaud(BAUD_RATE); //set baud rate
bensonsinsin998 0:858c61a9c2de 102 nh.initNode();
bensonsinsin998 0:858c61a9c2de 103
bensonsinsin998 0:858c61a9c2de 104 nh.subscribe(subsqu);
bensonsinsin998 0:858c61a9c2de 105 nh.subscribe(subcro);
bensonsinsin998 0:858c61a9c2de 106 nh.subscribe(subcir);
bensonsinsin998 0:858c61a9c2de 107 nh.subscribe(subtri);
howanglam3 3:759af41b06ee 108 nh.subscribe(subkeypad);
bensonsinsin998 0:858c61a9c2de 109 while(true) {
bensonsinsin998 0:858c61a9c2de 110 nh.spinOnce();
bensonsinsin998 0:858c61a9c2de 111 if(nh.connected())
bensonsinsin998 0:858c61a9c2de 112 myled = 1;
bensonsinsin998 0:858c61a9c2de 113 else
bensonsinsin998 0:858c61a9c2de 114 myled = 0;
bensonsinsin998 0:858c61a9c2de 115 }
bensonsinsin998 0:858c61a9c2de 116 }