ハヤト タジリ
For ros melodic
Dependencies: mbed MAIDROBO ros_lib_melodic
Revision 0:f9a1c6f13adc, committed 2020-02-27
- Comitter:
- tajiri1999
- Date:
- Thu Feb 27 12:36:39 2020 +0000
- Commit message:
- test
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Thu Feb 27 12:36:39 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tajiri1999/code/MAIDROBO/#1e47f83b161c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Feb 27 12:36:39 2020 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+#include "Motor.h"
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Twist.h>
+
+#define SPEED 100 //set speed target = 1m/s
+#define AXLE 2.0 //
+#define WHEELS_SIZE 0.05//5cm
+
+//Motor
+Motor motor( PA_12, PA_11, PA_5, PB_1, PB_2, PA_9, PB_15, PB_14, PB_13, PB_4, PA_8, PB_10);
+
+//Switch
+DigitalIn sw_start(PD_2); //program start switch
+DigitalIn sw_reset(PC_12);
+
+//prototype
+void MotorCmdCallback(const geometry_msgs::Twist& msg);
+
+//instance
+ros::NodeHandle nh;
+ros::Subscriber<geometry_msgs::Twist> sub("robot/cmd_vel",&MotorCmdCallback);
+
+
+int main()
+{
+//**************************************************************//
+////////////////////////initialize setting////////////////////////
+//**************************************************************//
+ /*Ros setting*/
+ nh.getHardware()->setBaud(115200);
+ nh.initNode();
+ nh.subscribe(sub);
+
+ /*motor pwm frequency set*/
+ motor.Wheels(0,0);
+ motor.setPwmPeriod(0.00005);//20kHz
+
+ while (1) {
+//***************************************************************//
+////////////////MAIN LOOP////////////
+//***************************************************************//
+ nh.spinOnce();//毎回呼び出しが必要
+ if(sw_start.read() == 0) {
+ }
+ //wait_ms(10);
+ //motor.Wheels(0,0);
+ }
+ //return 0;
+}
+
+
+
+//**************************************************************//
+///////////////motor function///////////////////////////////////
+//**************************************************************//
+void MotorCmdCallback(const geometry_msgs::Twist& msg){
+ float v_l,v_r;//(m/s)
+ v_l = ((float)msg.angular.z*AXLE-2.0*(float)msg.linear.x)/(-2.0);//(m/s)
+ v_r = ((float)msg.angular.z*AXLE+2.0*(float)msg.linear.x)/2.0;//(m/s)
+ //v_l /= (WHEELS_SIZE * 2 * 3.14);//Wheel_speed(1/s)
+ //v_r /= (WHEELS_SIZE * 2 * 3.14);//Wheel_speed(1/s)
+ v_l *= SPEED;
+ v_r *= SPEED;
+ motor.Wheels(v_l,v_r);//-100~100
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Feb 27 12:36:39 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 Thu Feb 27 12:36:39 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e