Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@1:ac39b48026ca, 2021-03-13 (annotated)

Committer:
howanglam3
Date:
Sat Mar 13 05:26:36 2021 +0000
Revision:
1:ac39b48026ca
Parent:
0:858c61a9c2de
Child:
2:399bcafe6f36
try to add keypad function(not finished)

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