Revision 0:0846fa975a83, committed 2017-03-28

Comitter:

tea5062001

Date:

Tue Mar 28 13:14:57 2017 +0000

Commit message:

lab5

Changed in this revision

diff -r 000000000000 -r 0846fa975a83 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 28 13:14:57 2017 +0000
@@ -0,0 +1,318 @@
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Vector3.h>
+
+//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 Kp 0.006f
+#define Ki 0.02f
+
+Ticker timer1;
+// servo motor
+PwmOut servo_cmd(A0);
+// DC motor
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+// Motor1 sensor
+InterruptIn HallA(A1);
+InterruptIn HallB(A2);
+// Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+
+// 函式宣告
+void init_IO();
+void init_TIMER();
+void timer1_ITR();
+void init_CN();
+void CN_ITR();
+void init_PWM();
+
+// servo motor
+float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
+// 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
+int angle = 0;
+int counter;
+
+
+// Hall sensor
+int HallA_1_state = 0;
+int HallB_1_state = 0;
+int state_1 = 0;
+int state_1_old = 0;
+int HallA_2_state = 0;
+int HallB_2_state = 0;
+int state_2 = 0;
+int state_2_old = 0;
+
+int c = 0; 
+int d = 0; 
+
+// DC motor rotation speed control
+int speed_count_1 = 0;
+float rotation_speed_1 = 0.0;
+float rotation_speed_ref_1 = 0;
+float pwm1_duty = 0.5;
+float PI_out_1 = 0.0;
+float err_1 = 0.0;
+float ierr_1 = 0.0;
+int speed_count_2 = 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;
+
+//rosserial
+ros::NodeHandle nh;
+
+geometry_msgs::Twist vel_msg;
+ros::Publisher p("feedback_wheel_angularVel", &vel_msg);
+
+void messageCallback(const geometry_msgs::Vector3 &msg_receive)
+{
+    rotation_speed_ref_1 = -msg_receive.x;
+    rotation_speed_ref_2 = msg_receive.y;
+}
+
+ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel",messageCallback);
+
+int main()
+{
+    init_TIMER();
+    init_PWM();
+    init_CN();
+    
+    nh.initNode();
+    nh.subscribe(s);
+    nh.advertise(p);
+
+    while(1) 
+    {
+        vel_msg.linear.x = rotation_speed_ref_1;
+        vel_msg.linear.y = rotation_speed_1;
+        vel_msg.linear.z = 0;
+        
+        vel_msg.angular.x = rotation_speed_ref_2;
+        vel_msg.angular.y = rotation_speed_2;
+        vel_msg.angular.z = 0;
+        
+        p.publish(&vel_msg);
+        
+        nh.spinOnce();
+        wait_ms(20);
+    }
+}
+
+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()
+{
+    servo_cmd.period_ms(20);
+    servo_cmd.write(servo_duty);
+
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+}
+void init_CN()
+{
+    HallA.rise(&CN_ITR);
+    HallA.fall(&CN_ITR);
+    HallB.rise(&CN_ITR);
+    HallB.fall(&CN_ITR);
+
+    HallA_2.rise(&CN_ITR);
+    HallA_2.fall(&CN_ITR);
+    HallB_2.rise(&CN_ITR);
+    HallB_2.fall(&CN_ITR);
+}
+
+void CN_ITR()
+{
+    // motor1
+    HallA_1_state = HallA.read();
+    HallB_1_state = HallB.read();
+
+    ///code for state determination///
+
+   state_1 = 10*HallA_1_state + HallB_1_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
+   
+   if(state_1_old != state_1)
+   { 
+        if((state_1_old/10) == (state_1_old%10))
+        {
+            if((state_1%10) != (state_1_old%10))
+            {
+                speed_count_1++;
+            }
+            else if((state_1/10) != (state_1_old/10))
+            {
+                speed_count_1--;
+            }
+        }
+        else if((state_1_old/10) != (state_1_old%10))
+        {
+            if((state_1%10) != (state_1_old%10))
+            {
+                speed_count_1--;
+            }
+            else if((state_1/10) != (state_1_old/10))
+            {
+                speed_count_1++;
+            }    
+        }
+    
+        state_1_old = state_1;
+    }
+   
+
+    //////////////////////////////////
+
+    //forward : speed_count_1 + 1
+    //backward : speed_count_1 - 1
+
+
+    // motor2
+    HallA_2_state = HallA_2.read();
+    HallB_2_state = HallB_2.read();
+
+    ///code for state determination///
+
+   state_2 = 10*HallA_2_state + HallB_2_state;   //state = AB (ex:A=1,B=0, state_1 = 10)
+    
+    if(state_2_old != state_2)
+    {
+        if((state_2_old/10) == (state_2_old%10))
+        {
+            if((state_2%10) != (state_2_old%10))
+            {
+                speed_count_2++;
+            }
+            else if((state_2/10) != (state_2_old/10))
+            {
+                speed_count_2--;
+            }
+        }
+        else if((state_2_old/10) != (state_2_old%10))
+        {
+            if((state_2%10) != (state_2_old%10))
+            {
+                speed_count_2--;
+            }
+            else if((state_2/10) != (state_2_old/10))
+            {
+                speed_count_2++;
+            }    
+        }
+    
+        state_2_old = state_2;
+    }
+   
+
+ 
+    //////////////////////////////////
+
+    //forward : speed_count_2 + 1
+    //backward : speed_count_2 - 1
+}
+
+void timer1_ITR()
+{
+    // servo motor
+    ///code for sevo motor///
+    
+       counter = counter + 1;
+    
+    if(counter == 100)    
+    {
+        servo_duty = 0.069;       
+    }
+    if(counter == 200)    
+    {
+        servo_duty = 0.0763;        
+    }
+    if(counter == 300)    
+    {
+        servo_duty = 0.0837;        
+    }
+    if(counter == 400)    
+    {
+        servo_duty = 0.091;        
+    }
+    if(counter == 500)    
+    {
+        servo_duty = 0.0983;         
+    }
+    if(counter == 600)    
+    {
+        servo_duty = 0.106;     
+    }      
+ 
+    if(counter == 700)
+    {
+        servo_duty = 0.113;
+    }
+     if(counter > 700)
+    {
+      counter=0;
+    }
+    
+    servo_cmd.write(servo_duty);
+    
+    
+    /////////////////////////
+    
+    if(servo_duty >= 0.113f)servo_duty = 0.113;
+    else if(servo_duty <= 0.025f)servo_duty = 0.025;
+    servo_cmd.write(servo_duty);
+
+    // motor1
+    rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ;   //unit: rpm
+    speed_count_1 = 0;
+
+    ///PI controller for motor1///
+   
+    err_1 = rotation_speed_ref_1 - rotation_speed_1;
+    ierr_1 = ierr_1 + err_1*Ts;
+    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(PI_out_1 + 0.5f);
+    TIM1->CCER |= 0x4;
+
+    //motor2
+    rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    speed_count_2 = 0;
+
+    ///PI controller for motor2///
+    
+    err_2 = rotation_speed_ref_2 - rotation_speed_2;
+    ierr_2 = ierr_2 + err_2*Ts;
+    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(PI_out_2 + 0.5f);
+    TIM1->CCER |= 0x40;
+}
\ No newline at end of file

diff -r 000000000000 -r 0846fa975a83 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 28 13:14:57 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb
\ No newline at end of file

diff -r 000000000000 -r 0846fa975a83 ros_lib_indigo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_indigo.lib	Tue Mar 28 13:14:57 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/garyservin/code/ros_lib_indigo/#fd24f7ca9688