Diff: main.cpp

Revision:

0:f9a1c6f13adc

--- /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
+}
+