Revision 0:e27261bd5773, committed 2019-03-27

Comitter:

shadow103012033

Date:

Wed Mar 27 07:07:21 2019 +0000

Commit message:

TA code;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hallsensor_hardware_decoder.lib	Wed Mar 27 07:07:21 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/mobile_robot_tea/code/hallsensor_hardware_decoder/#c685617c45e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 27 07:07:21 2019 +0000
@@ -0,0 +1,176 @@
+#include "mbed.h"
+#include <ros.h>
+#include "hallsensor_software_decoder.h"
+#include <geometry_msgs/Twist.h>
+
+#define Ts 0.01f 
+
+Ticker timer1;
+
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+void init_TIMER();
+void init_PWM(); 
+void timer1_ITR();
+
+
+float rotation_speed_1 = 0.0;
+float rotation_speed_ref_1 = 0;
+float pwm1_duty = 0.5;
+double PI_out_1 = 0.0;
+float err_1 = 0.0;
+float ierr_1 = 0.0;
+float rotation_speed_2 = 0.0;
+float rotation_speed_ref_2 = -0;
+float pwm2_duty = 0.5;
+float PI_out_2 = 0.0;
+float err_2 = 0.0;
+float ierr_2 = 0.0;
+
+float r = 0.03; // wheel radius (m)
+float L = 0.27; // car width (m)
+
+float V_ref = 0; // (m/s) max: 0.25
+float V = 0;
+float W_ref = 0;  // (rad/s)
+float W = 0;
+
+
+float Kp = 0.008;
+float Ki = 0.02;
+
+//Serial pc(USBTX, USBRX);
+
+//rosserial
+ros::NodeHandle nh;
+
+geometry_msgs::Twist vel_msg;
+ros::Publisher p("feedback_Vel", &vel_msg);
+
+void messageCallback(const geometry_msgs::Twist &msg_receive)
+{
+    V_ref = 0.25*msg_receive.linear.x;
+    W_ref = msg_receive.angular.z;
+
+}
+
+ros::Subscriber<geometry_msgs::Twist> s("cmd_vel",messageCallback);
+
+int main()
+{
+    //pc.baud(9600);
+    init_TIMER();
+    init_PWM();
+    init_CN();
+    
+    nh.initNode();
+    nh.subscribe(s);
+    nh.advertise(p);
+    
+    while(1) {
+        V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f;
+        W = (-1.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f;
+    /*    pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2);
+        pc.printf("V = %f, W = %f\r\n", V,W);*/
+        
+        vel_msg.linear.x = V;
+        vel_msg.angular.z = W;
+        
+        p.publish(&vel_msg);
+        
+        nh.spinOnce();
+        
+        wait_ms(100);
+    }
+}
+
+
+
+void init_TIMER()
+{
+    timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
+}
+
+
+void init_PWM()
+{
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+}
+
+
+void timer1_ITR()
+{
+    
+    rotation_speed_ref_1 = (V_ref / r - L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f;
+    rotation_speed_ref_2 = (V_ref / r + L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f;
+    
+    
+    // motor1
+
+    rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
+    speed_count_1 = 0;
+    
+    ///PI controller for motor1///
+    
+    err_1 = rotation_speed_ref_1 - rotation_speed_1;
+    ierr_1 = ierr_1 + Ts * err_1;
+    if(ierr_1 > 50.0f)
+    {
+        ierr_1 = 50.0;
+    }
+    else if(ierr_1 < -50.0f)
+    {
+        ierr_1 = -50.0;
+    }
+    PI_out_1 = (Kp * err_1 + Ki * ierr_1);
+  
+    //////////////////////////////
+    
+    if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
+    else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 
+    pwm1_duty = PI_out_1 + 0.5f;   
+    pwm1.write(pwm1_duty);
+    TIM1->CCER |= 0x4;
+    
+   
+    
+    
+   //motor2
+  
+
+    rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f;   //unit: rpm
+    speed_count_2 = 0;
+
+    ///PI controller for motor2///
+    err_2 = rotation_speed_ref_2 - rotation_speed_2;
+    ierr_2 = ierr_2 + Ts * err_2;
+    if(ierr_2 > 50.0f)
+    {
+        ierr_2 = 50.0;
+    }
+    else if(ierr_2 < -50.0f)
+    {
+        ierr_2 = -50.0;
+    }
+    PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ;
+  
+    
+    //////////////////////////////
+    
+    if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
+    else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
+    pwm2_duty = PI_out_2 + 0.5f;
+    pwm2.write(pwm2_duty);
+    TIM1->CCER |= 0x40;
+    
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 27 07:07:21 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	Wed Mar 27 07:07:21 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f