Diff: main.cpp

Revision:

1:13ce5b28f6dd

Parent:

0:74ea99c4db88

--- a/main.cpp	Thu Feb 23 12:35:53 2017 +0000
+++ b/main.cpp	Tue Mar 21 05:36:09 2017 +0000
@@ -1,5 +1,5 @@
 #include "mbed.h"
-
+Serial pc(USBTX,USBRX);
 //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)
@@ -24,8 +24,10 @@
 void init_IO();
 void init_TIMER();
 void timer1_ITR();
-void init_CN();
-void CN_ITR();
+void init_CN_1();
+void init_CN_2();
+void CN_ITR_1();
+void CN_ITR_2();
 void init_PWM();
 
 // servo motor
@@ -53,18 +55,21 @@
 float ierr_1 = 0.0;
 int speed_count_2 = 0;
 float rotation_speed_2 = 0.0;
-float rotation_speed_ref_2 = 0;
+float rotation_speed_ref_2 = 150;
 float pwm2_duty = 0.5;
 float PI_out_2 = 0.0;
 float err_2 = 0.0;
 float ierr_2 = 0.0;
-
+float Kp = 0.4;
+float Ki = 0.1;
 int main()
 {
-    init_TIMER();
+    
+
     init_PWM();
-    init_CN();
-
+    init_CN_1();
+    init_CN_2();
+    init_TIMER();
     while(1) {
     }
 }
@@ -80,55 +85,40 @@
     servo_cmd.write(servo_duty);
 
     pwm1.period_us(50);
-    pwm1.write(0.5);
+    pwm1.write(0.5f);
     TIM1->CCER |= 0x4;
 
     pwm2.period_us(50);
-    pwm2.write(0.5);
+    pwm2.write(0.5f);
     TIM1->CCER |= 0x40;
 }
-void init_CN()
+void init_CN_1()
+{
+    HallA.rise(&CN_ITR_1);
+    HallA.fall(&CN_ITR_1);
+    HallB.rise(&CN_ITR_1);
+    HallB.fall(&CN_ITR_1);
+   
+}
+void init_CN_2()
 {
-    HallA.rise(&CN_ITR);
-    HallA.fall(&CN_ITR);
-    HallB.rise(&CN_ITR);
-    HallB.fall(&CN_ITR);
+    HallA_2.rise(&CN_ITR_2);
+    HallA_2.fall(&CN_ITR_2);
+    HallB_2.rise(&CN_ITR_2);
+    HallB_2.fall(&CN_ITR_2);
+    
+    }
 
-    HallA_2.rise(&CN_ITR);
-    HallA_2.fall(&CN_ITR);
-    HallB_2.rise(&CN_ITR);
-    HallB_2.fall(&CN_ITR);
+
+void CN_ITR_1()
+{
+    speed_count_1 = speed_count_1+1;
 }
 
-void CN_ITR()
+void CN_ITR_2()
 {
-    // motor1
-    HallA_1_state = HallA.read();
-    HallB_1_state = HallB.read();
-
-    ///code for state determination///
-
-
-
-    //////////////////////////////////
-
-    //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///
-
-
-
-    //////////////////////////////////
-
-    //forward : speed_count_2 + 1
-    //backward : speed_count_2 - 1
-}
+    speed_count_2 = speed_count_2+1;
+    }
 
 void timer1_ITR()
 {
@@ -145,6 +135,7 @@
 
     // motor1
     rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+  //  pc.printf("%f   ",rotation_speed_1);
     speed_count_1 = 0;
 
     ///PI controller for motor1///
@@ -153,25 +144,33 @@
     
     //////////////////////////////
     
+    /*
+    
     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 / 12.0f * 60.0f / 29.0f;   //unit: rpm
+   // rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    pc.printf("%d   \n\r",speed_count_2*100/12*60/29);
     speed_count_2 = 0;
 
     ///PI controller for motor2///
     
     
+    err_2 = Kp*(rotation_speed_ref_2-rotation_speed_2);
+    ierr_2 = ierr_2 +Ki*(rotation_speed_ref_2-rotation_speed_2)*0.01; 
+    PI_out_2 = (float)(err_2)*0.005;
     
     //////////////////////////////
-    
+
     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