yuto kawamura / Mbed 2 deprecated Nucleo_vr_servo_project

k

Dependencies:   mbed Servo ros_lib_melodic

main.cpp@0:be6abb948b16, 2020-05-13 (annotated)

Committer:
yuto17320508
Date:
Wed May 13 18:07:23 2020 +0000
Revision:
0:be6abb948b16
j

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuto17320508 0:be6abb948b16 1 #include "mbed.h"
yuto17320508 0:be6abb948b16 2 #include <ros.h>
yuto17320508 0:be6abb948b16 3 #include <geometry_msgs/Twist.h>
yuto17320508 0:be6abb948b16 4
yuto17320508 0:be6abb948b16 5 PwmOut servos[3] = {
yuto17320508 0:be6abb948b16 6 PwmOut(PA_5),
yuto17320508 0:be6abb948b16 7 PwmOut(PA_6),
yuto17320508 0:be6abb948b16 8 PwmOut(PA_7),
yuto17320508 0:be6abb948b16 9 };
yuto17320508 0:be6abb948b16 10
yuto17320508 0:be6abb948b16 11 double offset_deg_servo[3] = {
yuto17320508 0:be6abb948b16 12 0.0,
yuto17320508 0:be6abb948b16 13 -10.0,
yuto17320508 0:be6abb948b16 14 20.0
yuto17320508 0:be6abb948b16 15 };
yuto17320508 0:be6abb948b16 16
yuto17320508 0:be6abb948b16 17 ros::NodeHandle nh;
yuto17320508 0:be6abb948b16 18 geometry_msgs::Twist degrees;
yuto17320508 0:be6abb948b16 19 geometry_msgs::Twist pub_degrees;
yuto17320508 0:be6abb948b16 20
yuto17320508 0:be6abb948b16 21 //ros::Publisher deg_pub("mbed_degrees", &pub_degrees);
yuto17320508 0:be6abb948b16 22 double setLimitDegree(double input){
yuto17320508 0:be6abb948b16 23 if(input <= -90.0) return -90.0;
yuto17320508 0:be6abb948b16 24 else if(input >= 90.0) return 90.0;
yuto17320508 0:be6abb948b16 25 else return input;
yuto17320508 0:be6abb948b16 26 }
yuto17320508 0:be6abb948b16 27
yuto17320508 0:be6abb948b16 28 void messageCb( const geometry_msgs::Twist& degrees) {
yuto17320508 0:be6abb948b16 29
yuto17320508 0:be6abb948b16 30 // servo position determined by a pulsewidth between 1-2ms : -90 ~ 90
yuto17320508 0:be6abb948b16 31 servos[0].pulsewidth((setLimitDegree(degrees.linear.x + offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 32 servos[1].pulsewidth((setLimitDegree(degrees.linear.y + offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 33 servos[2].pulsewidth((setLimitDegree(-degrees.linear.z + offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 34
yuto17320508 0:be6abb948b16 35 pub_degrees = degrees;
yuto17320508 0:be6abb948b16 36 //deg_pub.publish(&pub_degrees);
yuto17320508 0:be6abb948b16 37 }
yuto17320508 0:be6abb948b16 38
yuto17320508 0:be6abb948b16 39 ros::Subscriber<geometry_msgs::Twist> sub("degrees", &messageCb );
yuto17320508 0:be6abb948b16 40
yuto17320508 0:be6abb948b16 41 int main() {
yuto17320508 0:be6abb948b16 42 for(int i = 0; i < 3; ++i){
yuto17320508 0:be6abb948b16 43 servos[i].period(0.020); // servo requires a 20ms period
yuto17320508 0:be6abb948b16 44 }
yuto17320508 0:be6abb948b16 45
yuto17320508 0:be6abb948b16 46 servos[0].pulsewidth((setLimitDegree(offset_deg_servo[0]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 47 servos[1].pulsewidth((setLimitDegree(offset_deg_servo[1]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 48 servos[2].pulsewidth((setLimitDegree(offset_deg_servo[2]) / 90.0) / 1000.0 + 0.0015);
yuto17320508 0:be6abb948b16 49
yuto17320508 0:be6abb948b16 50 nh.getHardware()->setBaud(115200);
yuto17320508 0:be6abb948b16 51 nh.initNode();
yuto17320508 0:be6abb948b16 52 nh.subscribe(sub);
yuto17320508 0:be6abb948b16 53 //
yuto17320508 0:be6abb948b16 54 //nh.advertise(deg_pub);
yuto17320508 0:be6abb948b16 55
yuto17320508 0:be6abb948b16 56 while(1){
yuto17320508 0:be6abb948b16 57 nh.spinOnce();
yuto17320508 0:be6abb948b16 58
yuto17320508 0:be6abb948b16 59 wait_ms(30);
yuto17320508 0:be6abb948b16 60 }
yuto17320508 0:be6abb948b16 61 }