Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@2:399bcafe6f36, 2021-03-26 (annotated)

Committer:
howanglam3
Date:
Fri Mar 26 17:03:52 2021 +0000
Revision:
2:399bcafe6f36
Parent:
1:ac39b48026ca
Child:
3:759af41b06ee
on going

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"
bensonsinsin998 0:858c61a9c2de 3
bensonsinsin998 0:858c61a9c2de 4 #include <ros.h>
bensonsinsin998 0:858c61a9c2de 5 #include "std_msgs/Bool.h"
howanglam3 2:399bcafe6f36 6 #include "geometry_msgs/Twist.h"
bensonsinsin998 0:858c61a9c2de 7
bensonsinsin998 0:858c61a9c2de 8 #define BAUD_RATE 115200
bensonsinsin998 0:858c61a9c2de 9
bensonsinsin998 0:858c61a9c2de 10 using namespace std;
bensonsinsin998 0:858c61a9c2de 11 using namespace std_msgs;
bensonsinsin998 0:858c61a9c2de 12 using namespace ros;
howanglam3 2:399bcafe6f36 13 using namespace geometry_msgs;
bensonsinsin998 0:858c61a9c2de 14
bensonsinsin998 0:858c61a9c2de 15 DigitalOut myled = LED1;
bensonsinsin998 0:858c61a9c2de 16 DigitalOut Refer_Volt_3_3 = PB_1;
bensonsinsin998 0:858c61a9c2de 17 DigitalOut pin_A = PA_10;
bensonsinsin998 0:858c61a9c2de 18 DigitalOut pin_B = PA_2;
bensonsinsin998 0:858c61a9c2de 19 DigitalOut pin_C = PA_3;
bensonsinsin998 0:858c61a9c2de 20
howanglam3 2:399bcafe6f36 21
howanglam3 2:399bcafe6f36 22
bensonsinsin998 0:858c61a9c2de 23 NodeHandle nh;
bensonsinsin998 0:858c61a9c2de 24
bensonsinsin998 0:858c61a9c2de 25 Servo servo = D6;
bensonsinsin998 0:858c61a9c2de 26
bensonsinsin998 0:858c61a9c2de 27 void messagesqu(const Bool& _msg){
bensonsinsin998 0:858c61a9c2de 28 bool check = _msg.data;
bensonsinsin998 0:858c61a9c2de 29 if(check)
bensonsinsin998 0:858c61a9c2de 30 pin_A = 1;
bensonsinsin998 0:858c61a9c2de 31 else
bensonsinsin998 0:858c61a9c2de 32 pin_A = 0;
bensonsinsin998 0:858c61a9c2de 33 }
bensonsinsin998 0:858c61a9c2de 34
bensonsinsin998 0:858c61a9c2de 35 void messagecro(const Bool& _msg){
bensonsinsin998 0:858c61a9c2de 36 bool check = _msg.data;
bensonsinsin998 0:858c61a9c2de 37 if(check)
bensonsinsin998 0:858c61a9c2de 38 pin_B = 1;
bensonsinsin998 0:858c61a9c2de 39 else
bensonsinsin998 0:858c61a9c2de 40 pin_B = 0;
bensonsinsin998 0:858c61a9c2de 41 }
bensonsinsin998 0:858c61a9c2de 42
howanglam3 1:ac39b48026ca 43
bensonsinsin998 0:858c61a9c2de 44 void messagecir(const Bool& _msg){
bensonsinsin998 0:858c61a9c2de 45 bool check = _msg.data;
howanglam3 1:ac39b48026ca 46 if(check && servo == 0) {
bensonsinsin998 0:858c61a9c2de 47 servo = 1.0;
howanglam3 1:ac39b48026ca 48 }
howanglam3 1:ac39b48026ca 49 else if(check && servo == 1)
howanglam3 1:ac39b48026ca 50 {
howanglam3 1:ac39b48026ca 51 servo = 0;
howanglam3 1:ac39b48026ca 52 }
bensonsinsin998 0:858c61a9c2de 53 }
bensonsinsin998 0:858c61a9c2de 54
bensonsinsin998 0:858c61a9c2de 55 void messagetri(const Bool& _msg){
bensonsinsin998 0:858c61a9c2de 56 bool check = _msg.data;
bensonsinsin998 0:858c61a9c2de 57 if(check)
bensonsinsin998 0:858c61a9c2de 58 pin_C = 1;
bensonsinsin998 0:858c61a9c2de 59 else
bensonsinsin998 0:858c61a9c2de 60 pin_C = 0;
bensonsinsin998 0:858c61a9c2de 61 }
bensonsinsin998 0:858c61a9c2de 62
howanglam3 2:399bcafe6f36 63 void messagekeypad (const Twist& _msg){
howanglam3 2:399bcafe6f36 64 float x = _msg.linear.x;
howanglam3 2:399bcafe6f36 65 float y = _msg.linear.y;
howanglam3 2:399bcafe6f36 66
howanglam3 2:399bcafe6f36 67 }
howanglam3 2:399bcafe6f36 68
bensonsinsin998 0:858c61a9c2de 69 Subscriber<Bool> subsqu("button_square", &messagesqu);
bensonsinsin998 0:858c61a9c2de 70 Subscriber<Bool> subcro("button_cross", &messagecro);
bensonsinsin998 0:858c61a9c2de 71 Subscriber<Bool> subcir("button_circle", &messagecir);
bensonsinsin998 0:858c61a9c2de 72 Subscriber<Bool> subtri("button_triangle", &messagetri);
howanglam3 2:399bcafe6f36 73 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
bensonsinsin998 0:858c61a9c2de 74
bensonsinsin998 0:858c61a9c2de 75 int main() {
bensonsinsin998 0:858c61a9c2de 76 myled = 0;
bensonsinsin998 0:858c61a9c2de 77 servo = 0;
bensonsinsin998 0:858c61a9c2de 78
bensonsinsin998 0:858c61a9c2de 79 nh.getHardware()->setBaud(BAUD_RATE);
bensonsinsin998 0:858c61a9c2de 80 nh.initNode();
bensonsinsin998 0:858c61a9c2de 81
bensonsinsin998 0:858c61a9c2de 82 nh.subscribe(subsqu);
bensonsinsin998 0:858c61a9c2de 83 nh.subscribe(subcro);
bensonsinsin998 0:858c61a9c2de 84 nh.subscribe(subcir);
bensonsinsin998 0:858c61a9c2de 85 nh.subscribe(subtri);
bensonsinsin998 0:858c61a9c2de 86
bensonsinsin998 0:858c61a9c2de 87 while(true) {
bensonsinsin998 0:858c61a9c2de 88 nh.spinOnce();
bensonsinsin998 0:858c61a9c2de 89
bensonsinsin998 0:858c61a9c2de 90 if(nh.connected())
bensonsinsin998 0:858c61a9c2de 91 myled = 1;
bensonsinsin998 0:858c61a9c2de 92 else
bensonsinsin998 0:858c61a9c2de 93 myled = 0;
bensonsinsin998 0:858c61a9c2de 94 }
bensonsinsin998 0:858c61a9c2de 95 }