annop niponsiritham / Mbed 2 deprecated mobile_robot_ver2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI PID ros_lib_kinetic MPU9250

Files at this revision

API Documentation at this revision

Revision 0:837a9278616f, committed 2019-12-16

Comitter:
BoGBoG
Date:
Mon Dec 16 14:04:06 2019 +0000
Commit message:
mobile robot ver 2

Changed in this revision

MPU9250.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
ros_lib_kinetic.lib Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r 837a9278616f MPU9250.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.lib	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/BoGBoG/code/MPU9250/#30b0f05d30a4

diff -r 000000000000 -r 837a9278616f PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r 837a9278616f QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/BoGBoG/code/QEI/#94ab0cebd88f

diff -r 000000000000 -r 837a9278616f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,278 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "ros/time.h"
+#include "geometry_msgs/Twist.h"
+#include "nav_msgs/Odometry.h"
+#include "tf/transform_broadcaster.h"
+#include "tf/tf.h"
+#include "PID.h"
+#include "MPU9250.h"
+
+//    
+//    1 \\-----// 2                    
+//        |   |                        
+//        |   | <--robot               
+//        |   |                        
+//    4 //-----\\ 3             
+//
+
+//-----------------------define pin-----------------------//
+
+#define RATE 0.01
+
+#define kp 1.0
+#define ki 0.005
+#define kd 0.0001
+
+DigitalOut dir1(PC_9);
+DigitalOut dir2(PE_6);
+DigitalOut dir3(PF_7);
+DigitalOut dir4(PB_15);
+
+PwmOut pul1(PC_8);
+PwmOut pul2(PE_5);
+PwmOut pul3(PF_8);
+PwmOut pul4(PC_6);
+
+DigitalOut led(LED1);
+
+MPU9250 mpu9250;
+
+//------------------------------------------------------//
+
+//-----------------------define value-----------------------//
+uint8_t whoAmI = 0;
+long time_loop = 0;
+
+float l     = 0.2;
+float w     = 0.1;
+float rad   = 0.03;
+
+float u1 = 0.0;
+float u2 = 0.0;
+float u3 = 0.0;
+float u4 = 0.0;
+
+float speed1;
+float speed2;
+float speed3;
+float speed4;
+
+double yaw_imu;
+double lastyaw_imu;
+double yaw_cal;
+double first_yaw;
+
+uint32_t looptime1 = 0;
+uint32_t holdtime = 0;
+
+//----------------------------------------------------------//
+
+
+//-----------------------cal omega wheel----------------------//
+
+void messageCb(const geometry_msgs::Twist& msg)
+{
+    
+    u1 = ( -((l+w)*msg.angular.z) + msg.linear.x - msg.linear.y )/rad;  //omega wheel in rad/sec
+    u2 = (  ((l+w)*msg.angular.z) + msg.linear.x + msg.linear.y )/rad;
+    u3 = (  ((l+w)*msg.angular.z) + msg.linear.x - msg.linear.y )/rad;
+    u4 = ( -((l+w)*msg.angular.z) + msg.linear.x + msg.linear.y )/rad;
+    
+    if(u1>=0)   {dir1 = 1;}     //dir1-->go forward
+    if(u1<0)    {dir1 = 0;}     //dir1-->go backward
+    if(u2>=0)   {dir2 = 1;}
+    if(u2<0)    {dir2 = 0;}
+    if(u3>=0)   {dir3 = 1;}
+    if(u3<0)    {dir3 = 0;}
+    if(u4>=0)   {dir4 = 1;}
+    if(u4<0)    {dir4 = 0;}
+    
+    u1 = abs(u1);
+    u2 = abs(u2);
+    u3 = abs(u3);
+    u4 = abs(u4);
+}
+
+//------------------------------------------------------------//
+
+//-----------------------encoder----------------------//
+
+QEI en1 (PB_8, PB_9, NC, 748.44);
+QEI en2 (PA_5, PA_6, NC, 748.44);
+QEI en3 (PD_14,PD_15, NC, 748.44);
+QEI en4 (PE_9, PE_11, NC, 748.44);
+
+//----------------------------------------------------//
+
+//-----------------------PID Controller----------------------//
+
+PID controller1( kp, ki, kd, RATE);
+PID controller2( kp, ki, kd, RATE);
+PID controller3( kp, ki, kd, RATE);
+PID controller4( kp, ki, kd, RATE);
+
+//-----------------------------------------------------------//
+
+//-----------------------drive----------------------//
+
+void drive()
+{
+    controller1.setSetPoint(u1);
+    controller2.setSetPoint(u2);
+    controller3.setSetPoint(u3);
+    controller4.setSetPoint(u4);
+    
+    controller1.setProcessValue(abs(en1.getVelosity()));
+    speed1 = controller1.compute();
+    pul1.pulsewidth_us(speed1);
+    
+    controller2.setProcessValue(abs(en2.getVelosity()));
+    speed2 = controller2.compute();
+    pul2.pulsewidth_us(speed2);
+    
+    controller3.setProcessValue(abs(en3.getVelosity()));
+    speed3 = controller3.compute();
+    pul3.pulsewidth_us(speed3);
+    
+    controller4.setProcessValue(abs(en4.getVelosity()));
+    speed4 = controller4.compute();
+    pul4.pulsewidth_us(speed4);
+}
+
+//--------------------------------------------------//
+
+ros::NodeHandle nh;
+
+geometry_msgs::Twist msg;
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
+
+nav_msgs::Odometry _odometry_msg;
+ros::Publisher odom("odom", &_odometry_msg);
+
+geometry_msgs::TransformStamped t;
+tf::TransformBroadcaster broadcaster;
+
+Ticker time_up;
+
+//-----------------------main function----------------------//
+
+int main()
+{
+    nh.getHardware()->setBaud(250000);
+    nh.initNode();
+    nh.subscribe(sub);
+    
+    nh.advertise(odom);
+    broadcaster.init(nh);  
+    t.header.frame_id = "odom";
+    t.child_frame_id = "base_footprint";
+    
+    _odometry_msg.header.frame_id = "odom";
+    _odometry_msg.child_frame_id = "base_footprint";
+    
+    pul1.period_us(1000);
+    pul2.period_us(1000);
+    pul3.period_us(1000);
+    pul4.period_us(1000);
+  
+    controller1.setInputLimits(0.0, 36.56);
+    controller2.setInputLimits(0.0, 36.56);
+    controller3.setInputLimits(0.0, 36.56);
+    controller4.setInputLimits(0.0, 36.56);
+
+    controller1.setOutputLimits(0.0, 1000.0);
+    controller2.setOutputLimits(0.0, 1000.0);
+    controller3.setOutputLimits(0.0, 1000.0);
+    controller4.setOutputLimits(0.0, 1000.0);
+
+    controller1.setBias(1.0);
+    controller2.setBias(1.0);
+    controller3.setBias(1.0);
+    controller4.setBias(1.0);
+  
+    controller1.setMode(0);
+    controller2.setMode(0);
+    controller3.setMode(0);
+    controller4.setMode(0);
+    
+    whoAmI = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+    mpu9250.resetMPU9250(); 
+    mpu9250.MPU9250SelfTest(SelfTest); 
+    mpu9250.getAres(); 
+    mpu9250.getGres(); 
+    mpu9250.getMres(); 
+    
+    mpu9250.calibrateMPU9250(gyroBias,accelBias);
+    mpu9250.initMPU9250();
+    mpu9250.initAK8963(magCalibration);
+    //mpu9250.magcalMPU9250(magBias, magScale);
+    
+    time_up.attach(&drive, RATE);
+    
+    int16_t accData[3];
+    int16_t gyroData[3];
+    int16_t magData[3];
+    
+    mpu9250.readAccelData(accData);
+    mpu9250.readGyroData(gyroData);
+    mpu9250.readMagData(magData);
+    
+    pitch = atan2 ((((*(accData + 1)*aRes)-accelBias[1])) ,( sqrt ((((((*accData)    *aRes)-accelBias[0])) * ((((*accData)    *aRes)-accelBias[0]))) + ((((*(accData + 2)*aRes)-accelBias[2])) * (((*(accData + 2)*aRes)-accelBias[2]))))));
+    roll = atan2(-((((*accData)    *aRes)-accelBias[0])) ,( sqrt(((((*(accData + 1)*aRes)-accelBias[1])) * (((*(accData + 1)*aRes)-accelBias[1]))) + ((((*(accData + 2)*aRes)-accelBias[2])) * (((*(accData + 2)*aRes)-accelBias[2]))))));
+    
+    float Yh = (((*(magData + 1)*mRes)-magBias[1]) * cos(roll)) - (((*(magData + 2)*mRes)-magBias[2]) * sin(roll));
+    float Xh = (((*(magData)*mRes)-magBias[0]) * cos(pitch))+(((*(magData + 1)*mRes)-magBias[1]) * sin(roll)*sin(pitch)) + (((*(magData + 2)*mRes)-magBias[2]) * cos(roll) * sin(pitch));
+
+    first_yaw = (atan2(Yh, Xh));
+    
+    while (1)
+    {
+        int16_t accData[3];
+        int16_t gyroData[3];
+        int16_t magData[3];
+        
+        mpu9250.readAccelData(accData);
+        mpu9250.readGyroData(gyroData);
+        mpu9250.readMagData(magData);
+        
+        pitch = atan2 ((((*(accData + 1)*aRes)-accelBias[1])) ,( sqrt ((((((*accData)    *aRes)-accelBias[0])) * ((((*accData)    *aRes)-accelBias[0]))) + ((((*(accData + 2)*aRes)-accelBias[2])) * (((*(accData + 2)*aRes)-accelBias[2]))))));
+        roll = atan2(-((((*accData)    *aRes)-accelBias[0])) ,( sqrt(((((*(accData + 1)*aRes)-accelBias[1])) * (((*(accData + 1)*aRes)-accelBias[1]))) + ((((*(accData + 2)*aRes)-accelBias[2])) * (((*(accData + 2)*aRes)-accelBias[2]))))));
+        
+        float Yh = (((*(magData + 1)*mRes)-magBias[1]) * cos(roll)) - (((*(magData + 2)*mRes)-magBias[2]) * sin(roll));
+        float Xh = (((*(magData)*mRes)-magBias[0]) * cos(pitch))+(((*(magData + 1)*mRes)-magBias[1]) * sin(roll)*sin(pitch)) + (((*(magData + 2)*mRes)-magBias[2]) * cos(roll) * sin(pitch));
+
+        yaw_imu =  (atan2(Yh, Xh));
+        yaw_cal = (yaw_imu - lastyaw_imu);
+        if(yaw_cal < -3.14159265359)
+        {
+            yaw += 6.28318530718;
+        } 
+        if(yaw_cal > 3.14159265359)
+        {
+            yaw -= 6.28318530718;
+        } 
+        yaw += yaw_cal;
+        lastyaw_imu = yaw_imu;
+        
+        _odometry_msg.header.stamp = nh.now();
+        _odometry_msg.pose.pose.position.x = (( en1.getPulses()+en2.getPulses()+en3.getPulses()+en4.getPulses())*0.008395042)*(rad/4);
+        _odometry_msg.pose.pose.position.y = ((-en1.getPulses()+en2.getPulses()-en3.getPulses()+en4.getPulses())*0.008395042)*(rad/4);
+        _odometry_msg.pose.pose.position.z = 0;
+        _odometry_msg.pose.pose.orientation = tf::createQuaternionFromYaw(yaw-first_yaw);
+        odom.publish(&_odometry_msg);   
+    
+        t.transform.translation.x = _odometry_msg.pose.pose.position.x;
+        t.transform.translation.y = _odometry_msg.pose.pose.position.y;
+        t.transform.translation.z = 0;
+        t.transform.rotation = _odometry_msg.pose.pose.orientation;
+        t.header.stamp = nh.now();
+        broadcaster.sendTransform(t);
+
+        nh.spinOnce();
+        wait_ms(200);
+
+
+    }
+}
+//----------------------------------------------------------//
\ No newline at end of file

diff -r 000000000000 -r 837a9278616f mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r 837a9278616f ros_lib_kinetic.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Mon Dec 16 14:04:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/BoGBoG/code/ros_lib_kinetic/#daf91334a889