Diff: main.cpp

Revision:

2:c95a3cba51e6

Parent:

1:657bebc0fe37

--- a/main.cpp	Tue Mar 21 14:32:42 2017 +0000
+++ b/main.cpp	Thu Mar 23 06:36:33 2017 +0000
@@ -6,6 +6,8 @@
 
 Serial pc(USBTX, USBRX);
 
+DigitalOut led1(PC_1);
+
 Ticker timer1;
 // servo motor
 PwmOut servo_cmd(A0);
@@ -32,7 +34,7 @@
 void init_PWM();
 
 // servo motor
-float servo_duty = 0.025;  // 0.069 +(0.088/180)*angle, -90<angle<90
+float servo_duty = 0.069;  // 0.069 +(0.088/180)*angle, -90<angle<90
 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
 int angle = 0;
 
@@ -63,20 +65,30 @@
 float ierr_2 = 0.0;
 
 //set parameters
-float kp = 0.4;
-float ki = 0.01;//0.0x
+float kp = 0.015;
+float ki = 0.11;
 
 int main()
 {
     //pc.printf("hello main()\n");
+    led1 = 0;
     init_TIMER();
     init_PWM();
     init_CN();
 
     while(1) {
+        //pc.printf("state_1 = %d \n", state_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);
+        //pc.printf("speed_count_1 = %d \n", speed_count_1);
+        //pc.printf("speed_count_2 = %d \n", speed_count_2);
+        //pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
+        //pc.printf("rotation_speed_1 = %f \n", rotation_speed_1);
+        //pc.printf("error_1 = %f \n\r", err_1);
+        //pc.printf("ierror_1 = %f \n\r", ierr_1);
+        //pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
+        //pc.printf("error_2 = %f \n\r", err_2);
+        //pc.printf("ierror_2 = %f \n\r", ierr_2);
+        //pc.printf("Wm1 = %f (rpm), Wm2 = %f (rpm)", rotation_speed_1, rotation_speed_2);
     }
 }
 
@@ -115,9 +127,11 @@
 
 void CN_ITR()
 {
+    
     // motor1
     HallA_1_state = HallA.read();
     HallB_1_state = HallB.read();
+    //led1 != led1; 
 
     ///code for state determination///
     //////////////////////////////////
@@ -145,13 +159,13 @@
     direction_1 = state_1 - state_1_old;
     if((direction_1 == -1) || (direction_1 == 3))
     {
-        //backward
-        speed_count_1 = speed_count_1 - 1;
+        //forward
+        speed_count_1 = speed_count_1 + 1;
     }
     else if((direction_1 == 1) || (direction_1 == -3))
     {
-        //forward
-        speed_count_1 = speed_count_1 + 1;
+        //backward
+        speed_count_1 = speed_count_1 - 1;
     }
     else
     {
@@ -196,13 +210,13 @@
     direction_2 = state_2 - state_2_old;
     if((direction_2 == 1) || (direction_2 == -3))
     {
-        //backward
-        speed_count_2 = speed_count_2 - 1;
+        //forward
+        speed_count_2 = speed_count_2 + 1;
     }
     else if((direction_2 == -1) || (direction_2 == 3))
     {
-        //forward
-        speed_count_2 = speed_count_2 + 1;
+        //backward
+        speed_count_2 = speed_count_2 - 1;
     }
     else
     {
@@ -240,9 +254,7 @@
     
     // 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///
     //////////////////////////////
@@ -250,40 +262,32 @@
     
     //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;
+    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);
-    
+  
     //////////////////////////////
     //////////////////////////////