Diff: main.cpp

Revision:

0:be6abb948b16

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 13 18:07:23 2020 +0000
@@ -0,0 +1,61 @@
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+
+PwmOut servos[3] = {
+    PwmOut(PA_5), 
+    PwmOut(PA_6),
+    PwmOut(PA_7),
+    };
+
+double offset_deg_servo[3] = {
+    0.0,
+    -10.0,
+    20.0
+};
+
+ros::NodeHandle nh;
+geometry_msgs::Twist degrees;
+geometry_msgs::Twist pub_degrees;
+
+//ros::Publisher deg_pub("mbed_degrees", &pub_degrees);
+double setLimitDegree(double input){
+    if(input <= -90.0) return -90.0;
+    else if(input >= 90.0) return 90.0;
+    else return input;
+}
+
+void messageCb( const geometry_msgs::Twist& degrees) {
+    
+    // servo position determined by a pulsewidth between 1-2ms : -90 ~ 90
+    servos[0].pulsewidth((setLimitDegree(degrees.linear.x + offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015); 
+    servos[1].pulsewidth((setLimitDegree(degrees.linear.y + offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015); 
+    servos[2].pulsewidth((setLimitDegree(-degrees.linear.z + offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015); 
+    
+    pub_degrees = degrees;
+    //deg_pub.publish(&pub_degrees);
+}
+ 
+ros::Subscriber<geometry_msgs::Twist> sub("degrees", &messageCb );
+
+int main() {
+    for(int i = 0; i < 3; ++i){
+        servos[i].period(0.020);          // servo requires a 20ms period   
+    }
+    
+    servos[0].pulsewidth((setLimitDegree(offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015); 
+    servos[1].pulsewidth((setLimitDegree(offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015); 
+    servos[2].pulsewidth((setLimitDegree(offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015); 
+    
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.subscribe(sub);
+    //
+    //nh.advertise(deg_pub);
+    
+     while(1){
+        nh.spinOnce();
+ 
+        wait_ms(30);
+    }
+}