Revision 0:c177452db984, committed 2020-11-02

Comitter:

sgrsn

Date:

Mon Nov 02 09:00:16 2020 +0000

Child:

1:b20c9bcfb254

Commit message:

First commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JY901.lib	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/JY901/#7e7dd6184774

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyOdometer.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,92 @@
+#ifndef MY_ODOMETER_H
+#define MY_ODOMETER_H
+
+#include "JY901.h"
+#include "define.h"
+
+class CountWheel
+{
+    public:
+    CountWheel(Timer *t)
+    {
+        _t = t;
+        _t -> start();
+    }
+    float getRadian(float frequency)
+    {
+        last_time = current_time;
+        current_time = _t -> read();
+        float dt = current_time - last_time;
+        float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
+        return delta_rad;
+    }
+    
+    private:
+    Timer *_t;
+    float last_time;
+    float current_time;
+};
+
+
+Timer tmp_t;
+CountWheel counter[4] = {CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t)};
+
+class MyOdometer
+{
+public:
+    float x;
+    float y;
+    float theta;
+    
+    float vel_x;
+    float vel_y;
+    float vel_theta;
+    
+    float wheel_frequency[4];
+
+    MyOdometer(JY901 *jy901)
+    {
+        x = 0;
+        y = 0;
+        theta = 0;
+        vel_x = 0;
+        vel_y = 0;
+        vel_theta = 0;
+        jy901_ = jy901;
+        vel_timer.start();
+    }
+    void update()
+    {
+        theta = jy901_ -> calculateAngleOnlyGyro() * DEG_TO_RAD;
+        //theta = jy901_ -> getZaxisAngle() * DEG_TO_RAD;
+        for(int i = 0; i < MOTOR_NUM; i++)
+        {
+            wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]);
+        }
+        float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        
+        x += dx * cos(theta) + dy * sin(-theta);
+        y += dy * cos(theta) + dx * sin(theta);
+        
+        vel_x = (-wheel_frequency[0] + wheel_frequency[1] + wheel_frequency[2] - wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
+                    
+        vel_y = (-wheel_frequency[0] - wheel_frequency[1] + wheel_frequency[2] + wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
+        
+        float current_time = vel_timer.read();
+        float dt = (current_time - last_time);
+        vel_theta = (theta - last_theta) / dt;
+        last_theta = theta; 
+        last_time = current_time;
+    }
+    
+private:
+    JY901 *jy901_;
+    Timer vel_timer;
+    float wheel_rad[MOTOR_NUM];
+    float last_time;
+    float last_theta;
+}; 
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/PID2/#873985df821c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/define.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,61 @@
+#ifndef MY_DEFINE_H
+#define MY_DEFINE_H
+
+#define PI 3.141592654
+
+float DEG_TO_RAD = PI/180.0;
+float RAD_TO_DEG = 180.0/PI;
+
+float wheel_d = 0.127;           // メカナム直径[mm]
+float wheel_r = 0.0635;
+float deg_per_pulse = 0.0072;   // ステッピングモータ(AR46SAK-PS50-1)
+
+// 目標位置到達判定
+float threshold_x     = 5; //[mm]
+float threshold_y     = 5; //[mm]
+float threshold_theta = 1 * DEG_TO_RAD;
+
+/*Common for Master and Slave***************/
+
+typedef enum
+{
+    RightForward    = 1,
+    LeftForward     = 2,
+    RightBack       = 3,
+    LeftBack        = 4
+}MotorPosition;
+
+typedef enum
+{
+    COAST   = 0,
+    BRAKE   = 1,    
+    CW      = 2,
+    CCW     = 3
+}MotorState;
+
+#define WHO_AM_I        0x00
+#define MY_IIC_ADDR     0x01
+#define MOTOR_DIR       0x04
+#define PWM_FREQUENCY   0x05
+
+
+/*Master only**************************************/
+
+#define MOTOR_NUM 4
+#define IIC_ADDR1        0xB0
+#define IIC_ADDR2        0xC0
+#define IIC_ADDR3        0xD0
+#define IIC_ADDR4        0xE0
+
+#define MaxFrequency   70000
+#define MinFrequency   0//50
+
+// Register Map from PC
+#define MARKER_X        0x05
+#define MARKER_Y        0x06
+#define MARKER_Z        0x07
+#define MARKER_ROLL     0x08
+#define MARKER_PITCH    0x09
+#define MARKER_YAW      0x010
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/device.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,13 @@
+#ifndef MY_DEVICE_H
+#define MY_DEVICE_H
+
+#include "i2cmaster.h"
+#include "JY901.h"
+#include "MyOdometer.h"
+
+i2c master(p28, p27);
+Timer timer;
+JY901 jy901(&master, &timer);
+MyOdometer my_odometry(&jy901);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cmaster.lib	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/i2cmaster/#bc6d5a6e9fe1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include <string> 
+#include "JY901.h"
+#include "PID.h"
+#include "define.h"
+
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/tf.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Pose2D.h>
+#include <std_msgs/Empty.h>
+#include <std_srvs/Trigger.h>
+#include <std_srvs/Empty.h>
+
+#include "robot_operator.h"
+#include "device.h"
+
+// To do
+// robot_vel_controll 速度がそのまま周波数指令になっているため単位がおかしい
+// あまり大きい値の指令を出すとmbed実行エラーが起きるLED1点滅
+
+// (my_odomで返すvel_xは差分を取るより周波数から計算した方がいいか。多分良い)
+//
+
+ros::NodeHandle mecanum_node;
+
+void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){
+    moveStop();
+    robot_vel_controll(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
+}
+
+void cmd_pose2d_callback(const geometry_msgs::Pose2D& cmd_pose2d){
+    //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+    moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+}
+
+/*
+void move_start_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
+  res.success = true;
+  res.message = "Moving...";
+  moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+}*/
+
+void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
+  res.success = true;
+  res.message = "Stop";
+  moveStop();
+}
+
+void odom_update()
+{
+    my_odometry.update();
+}
+
+//ros::ServiceServer<geometry_msgs::Pose2D,std_msgs::Empty> start_service("wheel/move_start", &move_start_service);
+ros::ServiceServer<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_service("wheel/move_stop", &move_stop_service);
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("wheel/cmd_vel", &cmd_vel_callback);
+ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback);
+
+int main() {
+    coastAllMotor();
+    mecanum_node.initNode();
+    mecanum_node.subscribe(cmd_vel_sub);
+    mecanum_node.subscribe(cmd_pose_sub);
+    //mecanum_node.advertiseService(start_service);
+    mecanum_node.advertiseService(stop_service);
+    
+    nav_msgs::Odometry odom;
+    ros::Publisher odom_pub("wheel/odom", &odom);
+    mecanum_node.advertise(odom_pub);
+    tf::TransformBroadcaster odom_broadcaster;
+    odom_broadcaster.init(mecanum_node);
+    
+    ros::Time current_time, last_time;
+    current_time = mecanum_node.now();
+    last_time = mecanum_node.now();
+    
+    Ticker ticker_odom;
+    ticker_odom.attach(&odom_update, 0.010);
+    
+    while (1) {
+        mecanum_node.spinOnce();
+        current_time = mecanum_node.now();
+        
+        wait_ms(500);
+        
+        //since all odometry is 6DOF we'll need a quaternion created from yaw
+        geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta);
+    
+        //first, we'll publish the transform over tf
+        geometry_msgs::TransformStamped odom_trans;
+        odom_trans.header.stamp = current_time;
+        odom_trans.header.frame_id = "odom";
+        odom_trans.child_frame_id = "base_link";
+    
+        odom_trans.transform.translation.x = my_odometry.x;
+        odom_trans.transform.translation.y = my_odometry.y;
+        odom_trans.transform.translation.z = 0.0;
+        odom_trans.transform.rotation = odom_quat;
+    
+        //send the transform
+        odom_broadcaster.sendTransform(odom_trans);
+    
+        //next, we'll publish the odometry message over ROS
+        nav_msgs::Odometry odom;
+        odom.header.stamp = current_time;
+        odom.header.frame_id = "odom";
+    
+        //set the position
+        odom.pose.pose.position.x = my_odometry.x;
+        odom.pose.pose.position.y = my_odometry.y;
+        odom.pose.pose.position.z = 0.0;
+        odom.pose.pose.orientation = odom_quat;
+    
+        //set the velocity
+        odom.child_frame_id = "base_link";
+        odom.twist.twist.linear.x = my_odometry.vel_x;
+        odom.twist.twist.linear.y = my_odometry.vel_y;
+        odom.twist.twist.angular.z = my_odometry.vel_theta;
+    
+        //publish the message
+        odom_pub.publish(&odom);
+    }
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 02 09:00:16 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/motor_controller.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "device.h"
+
+int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
+
+int controlMotor(int ch, int frequency)
+{    
+    int dir = COAST;
+    int size = 4;
+    if(ch < 0 || ch > 3)
+    {
+        //channel error
+        return 0;
+    }
+    else
+    {
+        // オドメトリ用に周波数を保存
+        my_odometry.wheel_frequency[ch] = frequency;
+        if(frequency > 0)
+        {
+            dir = CW;
+        }
+        else if(frequency < 0)
+        {
+            dir = CCW;
+            frequency = -frequency;
+        }
+        else
+        {
+            dir = BRAKE;
+        }
+        // 周波数制限 脱調を防ぐ
+        if(frequency > MaxFrequency) frequency = MaxFrequency;
+        //else if(frequency < MinFrequency) frequency = MinFrequency;
+        
+        master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
+        master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
+        
+        return frequency;
+    }   
+}
+
+
+void coastAllMotor()
+{
+    for(int i = 0; i < MOTOR_NUM; i++)
+    {
+        // オドメトリ用に周波数を保存
+        my_odometry.wheel_frequency[i] = 0;
+        master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot_operator.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,160 @@
+#include "motor_controller.h"
+#include "MyOdometer.h"
+#include "define.h"
+#include "PID.h"
+
+// 目標位置制御用の制御周期
+const float pose_tick = 0.010; // sec
+const float L = 0.233; //233 [mm]
+
+class MakePath
+{
+    public:
+    int target_x;
+    int target_y;
+    int target_z;
+    
+    MakePath()
+    {
+    }
+    int makePath(int targetX, int targetY, int targetZ, int x, int y, int z)
+    {
+        target_x = targetX;
+        target_y = targetY;
+        target_z = targetZ;
+        int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0;   //5mm間隔
+        //printf("num = %d\r\n", num);
+        for(int i = 1; i <= num; i++)
+        {
+            float a = float(i) / num;
+            PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD);
+            PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD);
+            PATH[i-1][2] = z + float(targetZ - z) * a;
+        }
+        if(num > 0)
+        {
+            PATH[num-1][0] = targetX;
+            PATH[num-1][1] = targetY;
+            PATH[num-1][2] = targetZ;
+        }
+        else
+        {
+            PATH[0][0] = targetX;
+            PATH[0][1] = targetY;
+            PATH[0][2] = targetZ;
+            num = 1;
+        }
+        return num;
+    }
+    
+    int getPathX(int i)
+    {
+        return PATH[i][0];
+    }
+    int getPathY(int i)
+    {
+        return PATH[i][1];
+    }
+    int getPathZ(int i)
+    {
+        return PATH[i][2];
+    }
+    
+    private:
+    int PATH[500][3];
+};
+
+Ticker ticker_pose_controll;
+MakePath myPath;
+int path = 0;
+int pathSize;
+
+
+Timer timer2;
+PID pid_x(&timer2);
+PID pid_y(&timer2);
+PID pid_theta(&timer2);
+
+void robot_vel_controll(float vel_x, float vel_y, float vel_theta);
+void moveStart(float pose_x, float pose_y, float pose_theta);
+void moveStop();
+void pose_controll();
+
+
+void robot_vel_controll(float vel_x, float vel_y, float vel_theta)
+{   
+    float v[MOTOR_NUM] = {};
+    // 各車輪の速度
+    v[0] = -vel_x - vel_y - L * vel_theta;  // m/sec
+    v[1] =  vel_x - vel_y - L * vel_theta;
+    v[2] =  vel_x + vel_y - L * vel_theta;
+    v[3] = -vel_x + vel_y - L * vel_theta;
+    
+    // 本当はこうするべき
+    // f = v * ppr / ( 2*PI * r);
+    // controlMotor(i, (int)f[i]);
+    
+    for(int i = 0; i < MOTOR_NUM; i++)
+    {
+        float f = v[i] / wheel_r * RAD_TO_DEG / deg_per_pulse;
+        controlMotor(i, (int)f);
+    }
+}
+
+void moveStart(float pose_x, float pose_y, float pose_theta)
+{
+    pid_x.setParameter(0.40, 0.0, 0.0);
+    pid_y.setParameter(0.40, 0.0, 0.0);
+    pid_theta.setParameter(0.10, 0.0, 0.0);
+    
+    pathSize = myPath.makePath(pose_x*1000,              pose_y*1000,   pose_theta*RAD_TO_DEG, 
+                                    my_odometry.x*1000, my_odometry.y*1000, my_odometry.theta*RAD_TO_DEG);
+    ticker_pose_controll.attach(&pose_controll, 0.010);
+    path = 0;
+}
+
+void moveStop()
+{
+    ticker_pose_controll.detach();
+}
+
+void pose_controll()
+{
+    float x_robot = my_odometry.x*1000;
+    float y_robot = my_odometry.y*1000;
+    float theta_robot = my_odometry.theta;
+    
+    // 目標位置判定
+    float err_x = myPath.target_x - x_robot;
+    float err_y = myPath.target_y - y_robot;
+    float err_theta = myPath.target_z - theta_robot;
+    
+    // 目標速度計算
+    // 慣性座標での速度
+    float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
+    float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
+    float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
+    path++;
+    if(path >= pathSize) path = pathSize-1;
+    
+    // ロボット座標での速度
+    float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
+    float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
+    
+    robot_vel_controll(x_dot/1000, y_dot/1000, theta_dot);
+    
+    // 目標位置到達した場合
+    if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
+    {
+        // 車輪を停止
+        coastAllMotor();
+        //pid_x.reset();
+        //pid_y.reset();
+        //pid_theta.reset();
+        wait_ms(500);
+        jy901.reset();
+        
+        // 制御を停止
+        ticker_pose_controll.detach();
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#5d429be7d0aa