Diff: main.cpp

Revision:

0:858c61a9c2de

Child:

1:ac39b48026ca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 05 05:04:38 2021 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "Servo.h"
+
+#include <ros.h>
+#include "std_msgs/Bool.h"
+
+#define BAUD_RATE 115200
+
+using namespace std;
+using namespace std_msgs;
+using namespace ros;
+
+DigitalOut myled = LED1;
+DigitalOut Refer_Volt_3_3 = PB_1;
+DigitalOut pin_A = PA_10;
+DigitalOut pin_B = PA_2;
+DigitalOut pin_C = PA_3;
+
+NodeHandle nh;
+
+Servo servo = D6;
+
+void messagesqu(const Bool& _msg){
+    bool check = _msg.data;
+    if(check)
+        pin_A = 1;
+    else
+        pin_A = 0;
+}
+
+void messagecro(const Bool& _msg){
+    bool check = _msg.data;
+    if(check)
+        pin_B = 1;
+    else
+        pin_B = 0;
+}
+
+void messagecir(const Bool& _msg){
+    bool check = _msg.data;
+    if(check)
+        servo = 1.0;
+    else
+        servo = 0;
+}
+
+void messagetri(const Bool& _msg){
+    bool check = _msg.data;
+    if(check)
+        pin_C = 1;
+    else
+        pin_C = 0;
+}
+
+Subscriber<Bool> subsqu("button_square", &messagesqu);
+Subscriber<Bool> subcro("button_cross", &messagecro);
+Subscriber<Bool> subcir("button_circle", &messagecir);
+Subscriber<Bool> subtri("button_triangle", &messagetri);
+
+
+int main() {
+    myled = 0;
+    servo = 0;
+    
+    nh.getHardware()->setBaud(BAUD_RATE);
+    nh.initNode();
+    
+    nh.subscribe(subsqu);
+    nh.subscribe(subcro);
+    nh.subscribe(subcir);
+    nh.subscribe(subtri);
+    
+    while(true) {
+        nh.spinOnce();
+        
+        if(nh.connected())
+            myled = 1;
+        else
+            myled = 0;
+    }
+}
\ No newline at end of file