Diff: main.cpp

Revision:

1:d24c3384bc59

Parent:

0:5e356103dcc7

diff -r 5e356103dcc7 -r d24c3384bc59 main.cpp
--- a/main.cpp	Thu Apr 12 09:59:57 2018 +0000
+++ b/main.cpp	Wed Apr 18 09:48:11 2018 +0000
@@ -1,29 +1,32 @@
 /* LAB DCMotor */
 #include "mbed.h"
- 
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Twist.h>
+
 //****************************************************************************** Define
 //The number will be compiled as type "double" in default
 //Add a "f" after the number can make it compiled as type "float"
 #define Ts 0.01f    //period of timer1 (s)
 #define Servo_Period 20
+#define pi 3.14159265f
 //****************************************************************************** End of Define
  
 //****************************************************************************** I/O
 //PWM
-Serial pc(USBTX, USBRX); // tx, rx
  
 //Dc motor
-PwmOut pwm1(D7);
+PwmOut pwm1(D7); 
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
 PwmOut pwm2n(A3);
 PwmOut servo(A0);
 //Motor1 sensor
-InterruptIn HallA_1(A1);
-InterruptIn HallB_1(A2);
+InterruptIn HallA_R(A1);
+InterruptIn HallB_R(A2);
 //Motor2 sensor
-InterruptIn HallA_2(D13);
-InterruptIn HallB_2(D12);
+InterruptIn HallA_L(D13);
+InterruptIn HallB_L(D12);
  
 //LED
 DigitalOut led1(A4);
@@ -31,6 +34,7 @@
  
 //Timer Setting
 Ticker timer;
+
 //****************************************************************************** End of I/O
  
 //****************************************************************************** Functions
@@ -44,48 +48,96 @@
 //****************************************************************************** Variables
 // Servo
 float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree
-// motor 1
-int8_t HallA_state_1 = 0;
-int8_t HallB_state_1 = 0;
-int8_t motor_state_1 = 0;
-int8_t motor_state_old_1 = 0;
-int count_1 = 0;
-float speed_1 = 0.0f;
-float v_ref_1 = -80.0f;
-float v_err_1 = 0.0f;
-float v_ierr_1 = 0.0f;
-float ctrl_output_1 = 0.0f;
+// motor right
+int8_t HallA_state_R = 0;
+int8_t HallB_state_R = 0;
+int8_t motor_state_R = 0;
+int8_t motor_state_old_R = 0;
+int count_R = 0;
+float rotation_R = 0.0f;
+float rotation_ref_R = 0.0f;
+float rotation_err_R = 0.0f;
+float rotation_ierr_R = 0.0f;
+float ctrl_output_R = 0.0f;
 float pwm1_duty = 0.0f;
-//motor 2
-int8_t HallA_state_2 = 0;
-int8_t HallB_state_2 = 0;
-int8_t motor_state_2 = 0;
-int8_t motor_state_old_2 = 0;
-int count_2 = 0;
-float speed_2 = 0.0f;
-float v_ref_2 = 150.0f;
-float v_err_2 = 0.0f;
-float v_ierr_2 = 0.0f;
-float ctrl_output_2 = 0.0f;
+//motor left
+int8_t HallA_state_L = 0;
+int8_t HallB_state_L = 0;
+int8_t motor_state_L = 0;
+int8_t motor_state_old_L= 0;
+int count_L = 0;
+float rotation_L = 0.0f;
+float rotation_ref_L = 0.0f;
+float rotation_err_L = 0.0f;
+float rotation_ierr_L = 0.0f;
+float ctrl_output_L = 0.0f;
 float pwm2_duty = 0.0f;
 //servo
 int i = 0;
 //****************************************************************************** End of Variables
- 
+
+//****************************************************************************** ros related function
+ros:: NodeHandle nh;
+
+geometry_msgs::Twist vel_msg;
+ros::Publisher vel_feedback("feedback_wheel_angularVel", &vel_msg);
+float v_right = 0.0f;
+float v_left = 0.0f;
+void messageCb(const geometry_msgs::Vector3& msg)
+{
+    v_right = msg.y; //right wheel speed m/s 
+    v_left = msg.x; //left wheel speed m/s
+    
+    rotation_ref_R = -v_right/0.03f*60.0f/(2*pi); //  m/s to RPM
+    rotation_ref_L = v_left/0.03f*60.0f/(2*pi); //  m/s to RPM
+    //Limit the speed
+    /*if(rotation_ref_R > 120.69f)
+    {
+        rotation_ref_R = 120.69f;   
+    }
+    else if(rotation_ref_R < -120.69f)
+    {
+        rotation_ref_R = -120.69;
+    }
+    if(rotation_ref_L > 120.69f)
+    {
+        rotation_ref_L = 120.69f;   
+    }
+    else if(rotation_ref_L < -120.69f)
+    {
+        rotation_ref_L = -120.69f;
+    }*/
+}
+ros::Subscriber<geometry_msgs::Vector3> cmd_sub("cmd_wheel_angularVel", messageCb);
+
+
+//****************************************************************************** End of ros related function
 //****************************************************************************** Main
 int main()
 {
     init_timer();
     init_PWM();
     init_CN();
+    
+    nh.initNode();
+    
+    //cmd_sub = nh.subscribe("cmd_wheel_angularVel",10, messageCb);
+    nh.subscribe(cmd_sub);
+    nh.advertise(vel_feedback);
+    
     while(1)
     {
-        pc.printf("**************************\n");       
-        pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1);
-        pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1);
-        pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2);
-        pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2);
- 
+        vel_msg.linear.x = rotation_ref_R;
+        vel_msg.linear.y = rotation_R;
+        vel_msg.linear.z = 0.0f;
+          
+        vel_msg.angular.x = rotation_ref_L;
+        vel_msg.angular.y = rotation_L;
+        vel_msg.angular.z = 0.0f;
+        
+        vel_feedback.publish(&vel_msg);
+        nh.spinOnce();
+        wait_ms(100);
     }
 }
 //****************************************************************************** End of Main
@@ -93,40 +145,43 @@
 //****************************************************************************** timer_interrupt
 void timer_interrupt()
 {   
-    // Motor1
-    speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
-    count_1 = 0;
+    // Motor R
+    rotation_R = (float)count_R * 100.0f / 12.0f * 60.0f / 29.0f; //17.24 hall count convert to RPM
+    count_R = 0;
     // Code for PI controller //
-    v_err_1 = v_ref_1 - speed_1;
-    v_ierr_1 += (v_err_1*Ts);
-    ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1; 
+    rotation_err_R = rotation_ref_R - rotation_R;
+    rotation_ierr_R += (rotation_err_R*Ts);
+    ctrl_output_R = 0.01f*rotation_err_R+ 0.2f*rotation_ierr_R; 
     ///////////////////////////
     
-    if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
-    else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
-    pwm1_duty = ctrl_output_1 + 0.5f;
+    if(ctrl_output_R >= 0.5f)ctrl_output_R = 0.5f;
+    else if(ctrl_output_R <= -0.5f)ctrl_output_R = -0.5f;
+    pwm1_duty = ctrl_output_R + 0.5f;
     pwm1.write(pwm1_duty);
     TIM1->CCER |= 0x4;
     
-    // Motor2
-    speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
-    count_2 = 0;
+    // Motor L
+    rotation_L = (float)count_L * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
+    count_L = 0;
     // Code for PI controller //
-    v_err_2 = v_ref_2 - speed_2;
-    v_ierr_2 += (v_err_2*Ts);
-    ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2;
+    rotation_err_L = rotation_ref_L - rotation_L;
+    rotation_ierr_L += (rotation_err_L*Ts);
+    ctrl_output_L = 0.01f*rotation_err_L + 0.2f*rotation_ierr_L;
     ///////////////////////////      
-    if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
-    else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
-    pwm2_duty = ctrl_output_2 + 0.5f;
+    if(ctrl_output_L >= 0.5f)ctrl_output_L = 0.5f;
+    else if(ctrl_output_L <= -0.5f)ctrl_output_L = -0.5f;
+    pwm2_duty = ctrl_output_L + 0.5f;
     pwm2.write(pwm2_duty);
     TIM1->CCER |= 0x40;
-    
-    if(v_ierr_1 > 5)
-        v_ierr_1 = 0;
-    if(v_ierr_2 > 8)
-        v_ierr_2 = 0;
-        
+    /*
+    if(rotation_ierr_R > 100 || rotation_ierr_R < -100)
+    {
+        rotation_ierr_R = 0;
+    }
+    if(rotation_ierr_L > 100 || rotation_ierr_L < -100)
+    {
+        rotation_ierr_L = 0;
+    } */   
     //Servo
     if(i==100) 
     {
@@ -145,80 +200,78 @@
     {
         i++;
     }
+
 }
 //****************************************************************************** End of timer_interrupt
  
 //****************************************************************************** CN_interrupt
 void CN_interrupt()
 {
-    // Motor1
+    // Motor Right
     // Read the current status of hall sensor
-    HallA_state_1 = HallA_1.read();
-    HallB_state_1 = HallB_1.read();
+    HallA_state_R = HallA_R.read();
+    HallB_state_R = HallB_R.read();
      
    ///code for state determination///
-    if(HallA_state_1 == 0 && HallB_state_1 == 0)
-        motor_state_1 = 1;
-    else if(HallA_state_1 == 0 && HallB_state_1 == 1)
-        motor_state_1 = 2;
-    else if(HallA_state_1 == 1 && HallB_state_1 == 1)
-        motor_state_1 = 3;
-    else if(HallA_state_1 == 1 && HallB_state_1 == 0)
-        motor_state_1 = 4;
+    if(HallA_state_R == 0 && HallB_state_R == 0)
+        motor_state_R = 1;
+    else if(HallA_state_R == 0 && HallB_state_R == 1)
+        motor_state_R = 2;
+    else if(HallA_state_R == 1 && HallB_state_R == 1)
+        motor_state_R = 3;
+    else if(HallA_state_R == 1 && HallB_state_R == 0)
+        motor_state_R = 4;
     
-    if(motor_state_old_1 != 0)
+    if(motor_state_old_R != 0)
     {
-        if(motor_state_old_1 < motor_state_1)
-            count_1 += 1;
-        else if(motor_state_old_1 > motor_state_1)
-            count_1 -= 1;
-        if(motor_state_old_1 == 4 && motor_state_1 == 1)
-            count_1 += 2;
-        if(motor_state_old_1 == 1 && motor_state_1 == 4)
-            count_1 -= 2;
+        if(motor_state_old_R < motor_state_R)
+        {
+            count_R += 1;
+            if(motor_state_old_R == 1 && motor_state_R == 4)
+                count_R -= 2;
+        }
+        else if(motor_state_old_R > motor_state_R)
+        {
+            count_R -= 1;
+            if(motor_state_old_R == 4 && motor_state_R == 1)
+                count_R += 2;
+        }
     }
-    motor_state_old_1 = motor_state_1;
+    motor_state_old_R = motor_state_R;
     //////////////////////////////////
-    
-    //Forward
-    //v1Count +1
-    //Inverse
-    //v1Count -1
-        
-    // Motor2
+
+    // Motor Left
     // Read the current status of hall sensor
-    HallA_state_2 = HallA_2.read();
-    HallB_state_2 = HallB_2.read();
+    HallA_state_L = HallA_L.read();
+    HallB_state_L = HallB_L.read();
  
     ///code for state determination///
-    if(HallA_state_2 == 0 && HallB_state_2 == 0)
-        motor_state_2 = 1;
-    else if(HallA_state_2 == 0 && HallB_state_2 == 1)
-        motor_state_2 = 2;
-    else if(HallA_state_2 == 1 && HallB_state_2 == 1)
-        motor_state_2 = 3;
-    else if(HallA_state_2 == 1 && HallB_state_2 == 0)
-        motor_state_2 = 4;
+    if(HallA_state_L == 0 && HallB_state_L == 0)
+        motor_state_L = 1;
+    else if(HallA_state_L == 0 && HallB_state_L == 1)
+        motor_state_L = 2;
+    else if(HallA_state_L == 1 && HallB_state_L == 1)
+        motor_state_L = 3;
+    else if(HallA_state_L == 1 && HallB_state_L == 0)
+        motor_state_L = 4;
     
-    if(motor_state_old_2 != 0)
+    if(motor_state_old_L != 0)
     {
-        if(motor_state_old_2 < motor_state_2)
-            count_2 += 1;
-        else if(motor_state_old_2 > motor_state_2)
-            count_2 -= 1;
-        if(motor_state_old_2 == 4 && motor_state_2 == 1)
-            count_2 += 2;
-        if(motor_state_old_2 == 1 && motor_state_2 == 4)
-            count_2 -= 2;
+        if(motor_state_old_L < motor_state_L)
+        {
+            count_L += 1;
+            if(motor_state_old_L == 1 && motor_state_L == 4)
+                count_L -= 2;
+        }
+        else if(motor_state_old_L > motor_state_L)
+        {
+            count_L -= 1;
+            if(motor_state_old_L == 4 && motor_state_L == 1)
+                count_L += 2;
+        } 
     }
-    motor_state_old_2 = motor_state_2;
-    
+    motor_state_old_L = motor_state_L;
     //////////////////////////////////
-    
-    //Forward
-    //v2Count +1
-    //Inverse
-    //v2Count -1
 }
 //****************************************************************************** End of CN_interrupt
  
@@ -248,22 +301,22 @@
 //****************************************************************************** init_CN
 void init_CN()
 {
-    // Motor1
-    HallA_1.rise(&CN_interrupt);
-    HallA_1.fall(&CN_interrupt);
-    HallB_1.rise(&CN_interrupt);
-    HallB_1.fall(&CN_interrupt);
+    // Motor Right
+    HallA_R.rise(&CN_interrupt);
+    HallA_R.fall(&CN_interrupt);
+    HallB_R.rise(&CN_interrupt);
+    HallB_R.fall(&CN_interrupt);
     
-    HallA_state_1 = HallA_1.read();
-    HallB_state_1 = HallB_1.read();
+    HallA_state_R = HallA_R.read();
+    HallB_state_R = HallB_R.read();
     
-    // Motor2
-    HallA_2.rise(&CN_interrupt);
-    HallA_2.fall(&CN_interrupt);
-    HallB_2.rise(&CN_interrupt);
-    HallB_2.fall(&CN_interrupt);
+    // Motor Left
+    HallA_L.rise(&CN_interrupt);
+    HallA_L.fall(&CN_interrupt);
+    HallB_L.rise(&CN_interrupt);
+    HallB_L.fall(&CN_interrupt);
     
-    HallA_state_2 = HallA_2.read();
-    HallB_state_2 = HallB_2.read();
+    HallA_state_L = HallA_L.read();
+    HallB_state_L = HallB_L.read();
 }
-//****************************************************************************** End of init_CN
\ No newline at end of file
+//****************************************************************************** End of init_CN