annop niponsiritham
mechanum wheel drive
Dependencies: mbed QEI PID ros_lib_kinetic
Revision 0:e5d268f797d7, committed 2019-12-06
- Comitter:
- BoGBoG
- Date:
- Fri Dec 06 09:34:30 2019 +0000
- Commit message:
- mobile robot
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Dec 06 09:34:30 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Dec 06 09:34:30 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/BoGBoG/code/QEI/#94ab0cebd88f
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Dec 06 09:34:30 2019 +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_kinetic.lib Fri Dec 06 09:34:30 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/BoGBoG/code/ros_lib_kinetic/#f0984c3e4b1c