Diff: main.cpp

Revision:

1:b222972c5930

Parent:

0:74ea99c4db88

--- a/main.cpp	Thu Feb 23 12:35:53 2017 +0000
+++ b/main.cpp	Tue Mar 21 12:18:09 2017 +0000
@@ -1,5 +1,8 @@
 #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)
@@ -46,21 +49,27 @@
 // DC motor rotation speed control
 int speed_count_1 = 0;
 float rotation_speed_1 = 0.0;
-float rotation_speed_ref_1 = 0;
+float rotation_speed_ref_1 = 150;
 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 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 Ki = 0.02;
+float Kp = 0.7;
+
 int main()
 {
+ //    FILE *fp = fopen("C:/Users/user/Desktop/out.txt", "w");  // Open "out.txt" on the local file system for writing
+    
+    
     init_TIMER();
     init_PWM();
     init_CN();
@@ -80,11 +89,11 @@
     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()
@@ -101,33 +110,227 @@
 }
 
 void CN_ITR()
-{
-    // motor1
+{ // motor1
     HallA_1_state = HallA.read();
     HallB_1_state = HallB.read();
+    
+    // motor2
+    HallA_2_state = HallA_2.read();
+    HallB_2_state = HallB_2.read();
 
     ///code for state determination///
-
+    
+    // state determine (hall1) //
+    
+    if(HallA_1_state==0 && HallB_1_state ==0)
+    {
+      state_1 = 0;  
+       
+     }
+    
+    else if(HallA_1_state==1 && HallB_1_state ==0)
+     {
+        state_1 = 1;
+      }  
+       
+    else if(HallA_1_state==0 && HallB_1_state ==1)
+     {
+        state_1 = 2;
+      } 
+      
+    else if(HallA_1_state==1 && HallB_1_state ==1)
+     {
+        state_1 = 3;
+      }  
+    //   end state determine  of 1 //
+    
+    //  foward  and backward determine  of 1// 
+    
+    switch(state_1)
+    {
+        case 0 :
+        
+        if(state_1_old == 1)
+        {
+            speed_count_1 ++;    // foward
+        }
+        
+        else if(state_1_old == 2)
+        {
+            speed_count_1 --;  //        reverse
+        }
+        else{}
+        
+        break;
+        case 1 :
+        
+        if(state_1_old == 3)
+        {
+            speed_count_1 ++;    // foward
+        }
+        
+        else if(state_1_old == 0)
+        {
+            speed_count_1 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        case 2 :
+        
+        if(state_1_old == 0)
+        {
+            speed_count_1 ++;    // foward
+        }
+        
+        else if(state_1_old == 3)
+        {
+            speed_count_1 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        case 3 :
+        
+        if(state_1_old == 2)
+        {
+            speed_count_1 ++;    // foward
+        }
+        
+        else if(state_1_old == 1)
+        {
+            speed_count_1 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        
+     }
+    
+    
+    
+    //  end  foward  and reverse determine  of 1 //
+     
+    //  update//
+    state_1_old = state_1;
+    // end update //
+    
+    
+    
+    
+    // state determine (hall2) //
+    
+    
+    if(HallA_2_state==0 && HallB_2_state ==0)
+    {
+      state_2 = 0;  
+       
+     }
+    
+    else if(HallA_2_state==1 && HallB_2_state ==0)
+     {
+        state_2 = 1;
+      }  
+       
+    else if(HallA_2_state==0 && HallB_2_state ==1)
+     {
+        state_2 = 2;
+      } 
+      
+    else if(HallA_2_state==1 && HallB_2_state ==1)
+     {
+        state_2 = 3;
+      }  
+    //   end state determine  of 2//
+    
+    //  foward  and reverse determine  of 2// 
+    
+    switch(state_2)
+    {
+        case 0 :
+        
+        if(state_2_old == 1)
+        {
+            speed_count_2 ++;    // foward
+        }
+        
+        else if(state_2_old == 2)
+        {
+            speed_count_2 --;  //        reverse
+        }
+        else{}
+        
+        break;
+        case 1 :
+        
+        if(state_2_old == 3)
+        {
+            speed_count_2 ++;    // foward
+        }
+        
+        else if(state_2_old == 0)
+        {
+            speed_count_2 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        case 2 :
+        
+        if(state_2_old == 0)
+        {
+            speed_count_2 ++;    // foward
+        }
+        
+        else if(state_2_old == 3)
+        {
+            speed_count_2 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        case 3 :
+        
+        if(state_2_old == 2)
+        {
+            speed_count_2 ++;    // foward
+        }
+        
+        else if(state_2_old == 1)
+        {
+            speed_count_2 --;  //        reverse
+        }
+        
+        else{}
+        
+        break;
+        
+     }
+    
+    
+    
+    //  end  foward  and backward determine  of 1 //
+     
+    //  update//
+    state_2_old = state_2;
+    // end update //
+    
+    
+    
 
 
     //////////////////////////////////
 
     //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
+    
 }
 
 void timer1_ITR()
@@ -144,11 +347,16 @@
     servo_cmd.write(servo_duty);
 
     // motor1
-    rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    rotation_speed_1 = speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    pc.printf("%d\r\n",speed_count_1);
+    
     speed_count_1 = 0;
 
     ///PI controller for motor1///
+    err_1 = Kp *(rotation_speed_ref_1 - rotation_speed_1);
+    ierr_1 = ierr_1 + Ki*(rotation_speed_ref_1- rotation_speed_1)*0.01;
     
+    PI_out_1 = (err_1 + ierr_1)*0.002;
     
     
     //////////////////////////////
@@ -161,10 +369,15 @@
 
     //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 = float(Kp *(rotation_speed_ref_2 - rotation_speed_2));
+    ierr_2 = float(ierr_1 + Ki*(rotation_speed_ref_2- rotation_speed_2)*0.01);
     
+    PI_out_2 = float((rotation_speed_2+err_2 + ierr_2)*0.002);
     
     
     //////////////////////////////
@@ -174,4 +387,7 @@
     pwm2_duty = PI_out_2 + 0.5f;
     pwm2.write(pwm2_duty);
     TIM1->CCER |= 0x40;
+    
+    
+    
 }
\ No newline at end of file