Diff: main.cpp

Revision:

0:659581d9d096

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 21 18:41:54 2021 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "Servo.h"
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/UInt16.h>
+
+ 
+// ===== LED
+DigitalOut myled(LED2);
+DigitalOut led = LED1;
+
+// ===== SERVOS
+Servo grab(D3);
+Servo wrist(D6);
+Servo dropl(A6); // not plugged in yet
+Servo dropr(D10); // not plugged in yet
+Servo flag(D9);
+
+// ===== VARIABLES
+DigitalIn carousel(D11, PullDown);
+DigitalIn start(A5, PullDown);
+
+
+// ===== FUNCTIONS
+void grab_callback(const std_msgs::Float32& pos){
+    grab = pos.data;
+}
+
+void wrist_callback(const std_msgs::Float32& pos){
+    wrist = pos.data;
+}
+
+void dropl_callback(const std_msgs::Float32& pos){
+    dropl = pos.data;
+}
+
+void dropr_callback(const std_msgs::Float32& pos){
+    dropr = pos.data;
+}
+
+void flag_callback(const std_msgs::Float32& pos){
+    flag = pos.data;
+}
+
+
+// ===== ROSSERIAL SETUP
+ros::NodeHandle  nh;
+
+// Subscribers
+ros::Subscriber<std_msgs::Float32> grab_pos("hardware/servos/grab", grab_callback);
+//ros::Subscriber<std_msgs::Float32> dropl_pos("hardware/servos/dropl", wirst_callback);
+//ros::Subscriber<std_msgs::Float32> dropr_pos("hardware/servos/dropr", wirst_callback);
+ros::Subscriber<std_msgs::Float32> wrist_pos("hardware/servos/wrist", wrist_callback);
+ros::Subscriber<std_msgs::Float32> flag_pos("hardware/servos/flag", flag_callback);
+
+// Publishers
+std_msgs::UInt16 start_msg;
+ros::Publisher start_pub("hardware/switches/start", &start_msg);
+
+std_msgs::UInt16 carousel_msg;
+ros::Publisher carousel_pub("hardware/switches/carousel", &carousel_msg);
+
+// ===== MAIN
+int main() {
+    nh.initNode();
+    
+    nh.subscribe(grab_pos);
+    nh.subscribe(wrist_pos);
+    //nh.subscribe(dropr_pos);
+    //nh.subscribe(dropl_pos);
+    nh.subscribe(flag_pos);
+    
+    nh.advertise(start_pub);
+    nh.advertise(carousel_pub);
+
+    while (1) {
+        led = !led; // Toggle LED
+        
+        start_msg.data = start;
+        start_pub.publish( &start_msg );
+        
+        carousel_msg.data = carousel;
+        carousel_pub.publish( &carousel_msg );
+        
+        // Spin the rosserial node handler
+        nh.spinOnce();
+        
+        wait(0.02); // wait.....
+    }
+}