Diff: main.cpp

Revision:

0:68d5289b45cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 20 09:49:22 2018 +0000
@@ -0,0 +1,446 @@
+/* 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 PI    3.145926
+
+// PID parameter
+#define Kp_1  2.0f
+#define Ki_1  0.0f
+#define Kd_1  0.0f
+
+#define Kp_2  3.0f
+#define Ki_2  0.0f
+#define Kd_2  0.0f
+
+//****************************************************************************** End of Define
+ 
+//****************************************************************************** I/O
+//PWM
+//Dc motor
+PwmOut pwm1(D11);
+PwmOut pwm1n(D7);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+
+//LED
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+ 
+//Timer Setting
+Ticker timer;
+//****************************************************************************** End of I/O
+ 
+//****************************************************************************** Functions
+void init_timer(void);
+void init_CN(void);
+void init_PWM(void);
+void timer_interrupt(void);
+void CN_interrupt(void);
+//****************************************************************************** End of Functions
+ 
+//****************************************************************************** Variables
+// Servo
+float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
+
+// motor 1
+// decoder
+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;
+
+// velocity
+int count_1 = 0;
+float speed_1 = 0.0f;
+
+// control
+
+// u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
+float Ka_1 = Kp_1 + Ki_1 + Kd_1;
+float Kb_1 = -Kp_1 - 2*Kd_1;
+float Kc_1 = Kd_1;
+
+float v_ref_1 = 0.0f;           // r(k)
+float v_err_1 = 0.0f;           // e(k) = r(k) - v(k)
+float v_err_old_1 = 0.0f;       // e(k-1)
+float v_err_older_1 = 0.0f;     // e(k-2)
+float ctrl_output_1 = 0.0f;     // u(k)
+float ctrl_output_old_1 = 0.0f; // u(k-1)
+float pwm1_duty = 0.0f;         // duty = u(k) + 0.5
+
+
+// motor 2
+// decoder
+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;
+
+// velocity
+int count_2 = 0;
+float speed_2 = 0.0f;
+
+// control
+
+// u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
+float Ka_2 = Kp_2 + Ki_2 + Kd_2;
+float Kb_2 = -Kp_2 - 2*Kd_2;
+float Kc_2 = Kd_2;
+
+float v_ref_2 = 0.0f;           // r(k)
+float v_err_2 = 0.0f;           // e(k) = r(k) - v(k)
+float v_err_old_2 = 0.0f;       // e(k-1)
+float v_err_older_2 = 0.0f;     // e(k-2)
+float ctrl_output_2 = 0.0f;     // u(k)
+float ctrl_output_old_2 = 0.0f; // u(k-1)
+float pwm2_duty = 0.0f;         // duty = u(k) + 0.5
+
+
+
+
+//****************************************************************************** End of Variables
+ 
+//****************************************************************************** ROS
+/*
+ * **System Structure**
+ *
+ *  remote_car_node
+ *    pkg : robotics
+ *    pub : "/cmd_ang_vel"                  (Vector3)
+ *    sub : "/turtlebot_teleop/cmd_vel"     (Twist)
+ *
+ *  turtlebot_teleop_key
+ *    pkg : turtlebot_teleop
+ *    pub : "/turtlebot_teleop/cmd_vel"     (Twist)
+ *    sub : --
+ *
+ *  STM
+ *    pkg :
+ *    pub : "/feedback_wheel_ang_vel"       (Twist)
+ *    sub : "/cmd_ang_vel"                  (Vector3)
+ *
+ */
+
+
+// create a ROS node handle  
+ros::NodeHandle n;
+
+// pub a topic "/feedback_wheel_ang_vel"
+// with type "Twist"
+geometry_msgs::Twist wheel_ang_vel;
+ros::Publisher pub("/feedback_wheel_ang_vel", &wheel_ang_vel);
+
+// sub a topic "/cmd_ang_vel"
+// with type "Vector3"
+void messageCb(const geometry_msgs::Vector3 &cmd_received)
+{
+    v_ref_1 = cmd_received.x;
+    v_ref_2 = cmd_received.y;
+}
+ros::Subscriber<geometry_msgs::Vector3> sub("/cmd_ang_vel", messageCb);
+
+
+//****************************************************************************** End of ROS
+ 
+//****************************************************************************** Main
+int main()
+{
+    init_PWM();
+    init_timer();
+    init_CN();
+    
+    n.initNode();
+    n.subscribe(sub);
+    n.advertise(pub);
+    
+    while(1)
+    {
+        wheel_ang_vel.linear.x = v_ref_1;
+        wheel_ang_vel.linear.y = speed_1;
+        wheel_ang_vel.linear.z = 0.0f;
+        
+        wheel_ang_vel.angular.x = v_ref_2;
+        wheel_ang_vel.angular.y = speed_2;
+        wheel_ang_vel.angular.z = 0.0f;
+    
+        pub.publish(&wheel_ang_vel);
+        n.spinOnce();
+        wait_ms(500);
+    }
+}
+//****************************************************************************** End of Main
+ 
+ 
+ //***************************************************************************** init_timer
+void init_timer()
+{
+     timer.attach_us(&timer_interrupt, 100000);//100ms interrupt period (10 Hz)
+}
+//****************************************************************************** End of init_timer
+
+
+//****************************************************************************** timer_interrupt
+void timer_interrupt()
+{
+    
+    // Motor1
+    // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
+    speed_1 = (float)count_1 * 100.0f / 12.0f * 2 * PI / 29.0f;  // rad/s
+    count_1 = 0;
+    
+    
+    // Code for PID controller //
+    
+    
+    v_err_1 = v_ref_1 - speed_1 ;       // e(k) = r(k) - v(k)
+    
+    //    u(k)    =       u(k-1)      +   Ka*e(k)    +   Kb*e(k-1)      +    Kc*e(k-2)
+    ctrl_output_1 = ctrl_output_old_1 + Ka_1*v_err_1 + Kb_1*v_err_old_1 + Kc_1*v_err_older_1;
+    
+    v_err_older_1 = v_err_old_1;        // e(k-1)
+    v_err_old_1 = v_err_1;              // e(k-2)
+    ctrl_output_old_1 = ctrl_output_1;  // u(k-1)
+   
+ 
+    // limitter of ctrl_output
+    
+    if(ctrl_output_1 >= 0.5f)
+    {
+        ctrl_output_1 = 0.5f;
+    }
+    else if(ctrl_output_1 <= -0.5f)
+    {
+        ctrl_output_1 = -0.5f;
+    }
+    
+    // convert crtl_output to PWM signal
+    pwm1_duty = ctrl_output_1 + 0.5f;
+    pwm1.write(pwm1_duty);
+    TIM1->CCER |= 0x4;
+    
+    
+    // Motor2
+    // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
+    speed_2 = (float)count_2 * 100.0f / 12.0f * 2 * PI / 29.0f;  // rad/s
+    count_2 = 0;
+    
+    
+    // Code for PID controller //
+    
+    
+    v_err_2 = v_ref_2 - speed_2 ;       // e(k) = r(k) - v(k)
+    
+    //    u(k)    =       u(k-1)      +   Ka*e(k)    +   Kb*e(k-1)      +    Kc*e(k-2)
+    ctrl_output_2 = ctrl_output_old_2 + Ka_2*v_err_2 + Kb_2*v_err_old_2 + Kc_2*v_err_older_2;
+    
+    v_err_older_2 = v_err_old_2;        // e(k-1)
+    v_err_old_2 = v_err_2;              // e(k-2)
+    ctrl_output_old_2 = ctrl_output_2;  // u(k-1)
+   
+ 
+    // limitter of ctrl_output
+    
+    if(ctrl_output_2 >= 0.5f)
+    {
+        ctrl_output_2 = 0.5f;
+    }
+    else if(ctrl_output_2 <= -0.5f)
+    {
+        ctrl_output_2 = -0.5f;
+    }
+    
+    // convert crtl_output to PWM signal
+    pwm2_duty = ctrl_output_2 + 0.5f;
+    pwm2.write(pwm2_duty);
+    TIM1->CCER |= 0x40;
+    
+}
+//****************************************************************************** End of timer_interrupt
+ 
+//****************************************************************************** CN_interrupt
+void CN_interrupt_1()
+{
+    // Motor1
+    // Read the current status of hall sensor
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+     
+    ///code for state determination///
+    if(HallA_state_1 == 0)
+    {
+        if(HallB_state_1 == 0)
+        {// (0,0) -> 1
+            motor_state_1 = 1;
+        }
+        else
+        {// (0,1) -> 2
+            motor_state_1 = 2;   
+        }
+    }      
+    else
+    {
+        if(HallB_state_1 == 0)
+        {// (1,0) -> 4
+            motor_state_1 = 4;
+        }
+        else
+        {// (1,1) -> 3
+            motor_state_1 = 3;
+        }
+    }
+         
+    
+    // determind direction and count hall sensor changed //
+    switch(motor_state_1)
+    {
+        case 1:
+            if(motor_state_old_1 == 4)
+                count_1--;
+            else if(motor_state_old_1 == 2)
+                count_1++;
+            break;
+        case 2:
+            if(motor_state_old_1 == 1)
+                count_1--;
+            else if(motor_state_old_1 == 3)
+                count_1++;
+            break;
+        case 3:
+            if(motor_state_old_1== 2)
+                count_1--;
+            else if(motor_state_old_1 == 4)
+                count_1++;
+            break;
+        case 4:
+            if(motor_state_old_1 == 3)
+                count_1--;
+            else if(motor_state_old_1 == 1)
+                count_1++;
+            break;
+    }
+    
+    motor_state_old_1 = motor_state_1;
+    
+}
+
+void CN_interrupt_2()
+{
+    // Motor2
+    // Read the current status of hall sensor
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
+     
+    ///code for state determination///
+    if(HallA_state_2 == 0)
+    {
+        if(HallB_state_2 == 0)
+        {// (0,0) -> 1
+            motor_state_2 = 1;
+        }
+        else
+        {// (0,1) -> 2
+            motor_state_2 = 2;
+        }
+    }
+    else
+    {
+        if(HallB_state_2 == 0)
+        {// (1,0) -> 4
+            motor_state_2 = 4;
+        }
+        else
+        {// (1,1) -> 3
+            motor_state_2 = 3;
+        }
+    }
+      
+    // determind direction and count hall sensor changed //
+    switch(motor_state_2)
+    {
+        case 1:
+            if(motor_state_old_2 == 4)
+                count_2++;
+            else if(motor_state_old_2 == 2)
+                count_2--;
+            break;
+        case 2:
+            if(motor_state_old_2 == 1)
+                count_2++;
+            else if(motor_state_old_2 == 3)
+                count_2--;
+            break;
+        case 3:
+            if(motor_state_old_2== 2)
+                count_2++;
+            else if(motor_state_old_2 == 4)
+                count_2--;
+            break;
+        case 4:
+            if(motor_state_old_2 == 3)
+                count_2++;
+            else if(motor_state_old_2 == 1)
+                count_2--;
+            break; 
+    }
+    
+    motor_state_old_2 = motor_state_2;
+    
+}
+//****************************************************************************** End of CN_interrupt
+ 
+
+ 
+//****************************************************************************** init_PWM
+void init_PWM()
+{
+    // motor 1
+    pwm1.period_us(50);
+    pwm1.pulsewidth_us(25);
+    
+    TIM1->CCER |= 0x4;
+    
+    // motor 2
+    pwm2.period_us(50);
+    pwm2.pulsewidth_us(25);
+   
+    TIM1->CCER |= 0x40;
+}
+//****************************************************************************** End of init_PWM
+ 
+//****************************************************************************** init_CN
+void init_CN()
+{
+    // Motor1
+    HallA_1.rise(&CN_interrupt_1);
+    HallA_1.fall(&CN_interrupt_1);
+    HallB_1.rise(&CN_interrupt_1);
+    HallB_1.fall(&CN_interrupt_1);
+    
+    HallA_state_1 = HallA_1.read();
+    HallB_state_1 = HallB_1.read();
+    
+    // Motor2
+    HallA_2.rise(&CN_interrupt_2);
+    HallA_2.fall(&CN_interrupt_2);
+    HallB_2.rise(&CN_interrupt_2);
+    HallB_2.fall(&CN_interrupt_2);
+    
+    HallA_state_2 = HallA_2.read();
+    HallB_state_2 = HallB_2.read();
+
+}
+//****************************************************************************** End of init_CN