Diff: main.cpp

Revision:

2:a3c64321e9c2

Parent:

1:3b6c9baa7d0c

Child:

3:71a807b38a3e

--- a/main.cpp	Wed Mar 02 08:57:00 2016 +0000
+++ b/main.cpp	Thu Mar 03 15:04:40 2016 +0000
@@ -4,8 +4,8 @@
 //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 1.0f
-#define Ki 1.0f
+#define Kp 0.053f
+#define Ki 0.013f
 
 PwmOut servo(A0);
 PwmOut pwm1(D7);
@@ -26,16 +26,16 @@
 void flash(void);
 
 //Variable(s) for PI controller
-float ref = 0.45;       // 0.45 +(0.48/180.0)*angle, -90<angle<90
+float angle_ref = 0.0;  //unit in degree(s), range +-90 degrees
+float angle_read= 0.0;
+float angle_check;
 float err = 0.0;
 float ierr = 0.0;
 float PI_out = 0.0;
-float ad_read = 0.0;    // ADC value
-float ad_check;
 float pwm1_duty = 0.5;
 
 //Variable(s) for internal control
-float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90   
+float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
 
 int main (void)
 {    
@@ -50,8 +50,8 @@
 
 void timer1_interrupt(void)
 {
-    ad_read = adc.read();
-    ad_check = ad_read;
+    angle_read = (adc.read() - 0.45f) / 0.48f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
+    angle_check = angle_read;
     
     //////code for PI control//////
     
@@ -59,11 +59,12 @@
     
     
     
+    
     ////////////
     if(PI_out >= 0.5f)PI_out = 0.5;
     else if(PI_out <= -0.5f)PI_out = -0.5;
     pwm1_duty = PI_out + 0.5f;
-    if(ad_check > 0.69f || ad_check < 0.21f)pwm1_duty = 0.5;
+    if(angle_check > 100.0f || angle_check < -100.0f)pwm1_duty = 0.5;
     pwm1.write(pwm1_duty);
     TIM1->CCER |= 0x4;