Diff: main.cpp

Revision:

1:657bebc0fe37

Parent:

0:74ea99c4db88

Child:

2:c95a3cba51e6

--- a/main.cpp	Thu Feb 23 12:35:53 2017 +0000
+++ b/main.cpp	Tue Mar 21 14:32:42 2017 +0000
@@ -4,6 +4,8 @@
 //Add a "f" after the number can make it compiled as type "float"
 #define Ts 0.01f    //period of timer1 (s)
 
+Serial pc(USBTX, USBRX);
+
 Ticker timer1;
 // servo motor
 PwmOut servo_cmd(A0);
@@ -20,6 +22,7 @@
 InterruptIn HallA_2(D13);
 InterruptIn HallB_2(D12);
 
+
 // 函式宣告
 void init_IO();
 void init_TIMER();
@@ -46,31 +49,40 @@
 // 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.0;//150rpm
 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 = -80.0;//-80rpm
 float pwm2_duty = 0.5;
 float PI_out_2 = 0.0;
 float err_2 = 0.0;
 float ierr_2 = 0.0;
 
+//set parameters
+float kp = 0.4;
+float ki = 0.01;//0.0x
+
 int main()
 {
+    //pc.printf("hello main()\n");
     init_TIMER();
     init_PWM();
     init_CN();
 
     while(1) {
+        //pc.printf("state_2 = %d \n", state_2);
+        pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
+        pc.printf("rotation_speed_1 = %f \n", rotation_speed_1);
     }
 }
 
 void init_TIMER()
 {
+    //pc.printf("hello TIMER()\n");
     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
 }
 
@@ -89,6 +101,7 @@
 }
 void init_CN()
 {
+    //pc.printf("hello init_CN \n");
     HallA.rise(&CN_ITR);
     HallA.fall(&CN_ITR);
     HallB.rise(&CN_ITR);
@@ -107,71 +120,177 @@
     HallB_1_state = HallB.read();
 
     ///code for state determination///
-
-
-
+    //////////////////////////////////
     //////////////////////////////////
-
+    //determine the state
+    if((HallA_1_state == 0)&&(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;
+    }
+    else if((HallA_1_state == 1)&&(HallB_1_state ==0))
+    {
+        state_1 = 4;
+    }
+    
+    //forward or backward
+    int direction_1 = 0;
+    direction_1 = state_1 - state_1_old;
+    if((direction_1 == -1) || (direction_1 == 3))
+    {
+        //backward
+        speed_count_1 = speed_count_1 - 1;
+    }
+    else if((direction_1 == 1) || (direction_1 == -3))
+    {
+        //forward
+        speed_count_1 = speed_count_1 + 1;
+    }
+    else
+    {
+        //prevent initializing error
+        state_1_old = state_1;
+    }
+    
+    //change old state
+    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///
-
-
-
     //////////////////////////////////
-
+    /////////////////////////////////
+    //determine the state
+    if((HallA_2_state == 0)&&(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;
+    }
+    else if((HallA_2_state == 1)&&(HallB_2_state ==0))
+    {
+        state_2 = 4;
+    }
+    
+    //forward or backward
+    int direction_2 = 0;
+    direction_2 = state_2 - state_2_old;
+    if((direction_2 == 1) || (direction_2 == -3))
+    {
+        //backward
+        speed_count_2 = speed_count_2 - 1;
+    }
+    else if((direction_2 == -1) || (direction_2 == 3))
+    {
+        //forward
+        speed_count_2 = speed_count_2 + 1;
+    }
+    else
+    {
+        //prevent initializing error
+        state_2_old = state_2;
+    }
+    
+    //change old state
+    state_2_old = state_2;
+    //////////////////////////////////
+    //////////////////////////////////
     //forward : speed_count_2 + 1
     //backward : speed_count_2 - 1
 }
 
 void timer1_ITR()
 {
+    //pc.printf("hello timer1_ITR\n");
     // servo motor
     ///code for sevo motor///
-    
+    /////////////////////////
+    /////////////////////////   
+    //rotating angle for every call
+    angle = 15;
     
-    
+    //servo_duty output for every call
+    servo_duty = servo_duty + 0.088/180*angle/100;
+    /////////////////////////
     /////////////////////////
     
+    //protection code for servo
     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
+    //pc.printf("rotation_speed_1 = %d \n\r", rotation_speed_1);
     speed_count_1 = 0;
+    //pc.printf("speed_count_1 = %d \n\r",speed_count_1);
 
     ///PI controller for motor1///
-    
+    //////////////////////////////
+    //////////////////////////////
     
+    //PI control
+    err_1 = rotation_speed_ref_1 - rotation_speed_1;
+    //pc.printf("err_1 = %d \n\r", err_1);
+    ierr_1 += err_1*Ts;
+    PI_out_1 = kp * err_1 + ki * ierr_1;
+    //pc.printf("PI_out_1 = %d \n\r", PI_out_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;
+    //pc.printf("pwm1_duty = %d \n\r", pwm1_duty);
     pwm1.write(pwm1_duty);
     TIM1->CCER |= 0x4;
 
     //motor2
+    //pc.printf("rotation_speed_ref_2 = %f \n",rotation_speed_ref_2 );
     rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    //pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
     speed_count_2 = 0;
+    //pc.printf("speed_count_2 = %d \n", speed_count_2);
 
     ///PI controller for motor2///
-    
+    //////////////////////////////
+    //////////////////////////////
     
+    //PI control
+
+    err_2 = rotation_speed_ref_2 - rotation_speed_2;
+    //pc.printf("err_2 = %f \n\r", err_2);
+    ierr_2 += err_2 * Ts;
+    PI_out_2 = kp * err_2 + ki * ierr_2;
+    //pc.printf("PI_out_2 = %d \n\r", PI_out_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;
+    //pc.printf("pwm2_duty = %d \n\r", pwm2_duty);
     pwm2.write(pwm2_duty);
     TIM1->CCER |= 0x40;
 }
\ No newline at end of file