yuto kawamura
k
Dependencies: mbed Servo ros_lib_melodic
Revision 0:be6abb948b16, committed 2020-05-13
- Comitter:
- yuto17320508
- Date:
- Wed May 13 18:07:23 2020 +0000
- Commit message:
- j
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed May 13 18:07:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
--- /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);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed May 13 18:07:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_melodic.lib Wed May 13 18:07:23 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e