Diff: main.cpp

Revision:

0:e5d268f797d7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 06 09:34:30 2019 +0000
@@ -0,0 +1,237 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "ros/time.h"
+#include "sensor_msgs/JointState.h"
+#include "std_msgs/Float32.h"
+#include "geometry_msgs/Twist.h"
+#include "nav_msgs/Odometry.h"
+#include "tf/transform_broadcaster.h"
+#include "tf/tf.h"
+#include "PID.h"
+
+//    
+//    1 \\-----// 2                    
+//        |   |                        
+//        |   | <--robot               
+//        |   |                        
+//    4 //-----\\ 3             
+//
+
+//-----------------------define pin-----------------------//
+
+#define RATE 0.01
+
+#define kp 2.3
+#define ki 0.0
+#define kd 0.0
+
+DigitalOut dir1(PB_4);
+DigitalOut dir2(PB_5);
+DigitalOut dir3(PB_3);
+DigitalOut dir4(PA_10);
+
+PwmOut pul1(PC_7);
+PwmOut pul2(PA_9);
+PwmOut pul3(PA_8);
+PwmOut pul4(PB_10);
+
+DigitalOut led(LED1);
+//------------------------------------------------------//
+
+//-----------------------define value-----------------------//
+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 th =0;
+
+//----------------------------------------------------------//
+
+
+//-----------------------cal omega wheel----------------------//
+
+void messageCb(const geometry_msgs::Twist& msg)
+{
+    led = !led;
+    
+    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_7, PB_6, NC, 748.44);
+QEI en3 (PA_4, PB_0, NC, 748.44);
+QEI en4 (PC_1, PC_0, 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);
+
+sensor_msgs::JointState _joints_state_msg;
+ros::Publisher joint_states("joint_states", &_joints_state_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);
+    nh.advertise(joint_states);
+    broadcaster.init(nh);
+    
+    _joints_state_msg.header.frame_id = "base_footprint";
+    _joints_state_msg.name_length = 4;
+    char* names[4];
+    _joints_state_msg.name = names;
+    _joints_state_msg.name[0] = "wheel_front_left_joint";
+    _joints_state_msg.name[1] = "wheel_front_right_joint";
+    _joints_state_msg.name[2] = "wheel_rear_left_joint";
+    _joints_state_msg.name[3] = "wheel_rear_right_joint";
+    _joints_state_msg.position_length = 4;
+    double positions[4];
+    _joints_state_msg.position = positions;
+    
+    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);
+    
+    time_up.attach(&drive, RATE);
+    
+    while (1)
+    {
+        th = ((-en1.getPulses()+en2.getPulses()+en3.getPulses()-en4.getPulses())*0.00839504209)*(rad/(4*(l+w)));
+        
+        _joints_state_msg.header.stamp = nh.now(); 
+        _joints_state_msg.position[0] = en1.getPulses()*0.00839504209;//rad
+        _joints_state_msg.position[1] = en2.getPulses()*0.00839504209;//rad
+        _joints_state_msg.position[2] = en3.getPulses()*0.00839504209;//rad
+        _joints_state_msg.position[3] = en4.getPulses()*0.00839504209;//rad
+        joint_states.publish(&_joints_state_msg); 
+        
+        _odometry_msg.header.stamp = nh.now();
+        _odometry_msg.pose.pose.position.x = (( en1.getPulses()+en2.getPulses()+en3.getPulses()+en4.getPulses())*0.00839504209)*(rad/4);
+        _odometry_msg.pose.pose.position.y = ((-en1.getPulses()+en2.getPulses()-en3.getPulses()+en4.getPulses())*0.00839504209)*(rad/4);
+        _odometry_msg.pose.pose.position.z = 0;
+        _odometry_msg.pose.pose.orientation = tf::createQuaternionFromYaw(th);
+        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.rotation = _odometry_msg.pose.pose.orientation;
+        t.header.stamp = nh.now();
+        broadcaster.sendTransform(t);      
+        
+        nh.spinOnce();
+        
+        wait_ms(400);
+    }
+}
+//----------------------------------------------------------//
\ No newline at end of file