Diff: main.cpp

Revision:

3:237f0ac0ed1b

Parent:

2:cca4199fa834

Child:

4:5faca3dd2acc

--- a/main.cpp	Wed Mar 09 07:08:15 2016 +0000
+++ b/main.cpp	Thu Mar 10 04:41:44 2016 +0000
@@ -12,6 +12,8 @@
 #define INVERSE 1
 
 // variable declaration
+// COMMUNICATION
+Serial PC(USBTX,USBRX);
 // PWM
 PwmOut servo_Pwm(PA_0);
 PwmOut PI_Ctrl_Pwm(PA_8);
@@ -25,12 +27,9 @@
 Ticker timer_Ctrl;
 //Ticker timer_io;
 // GLOBAL
-float ang_cmd = 0;
-volatile float ang_read = 0;
+volatile float ang_cmd = 0;
 volatile float cur_angle = 0;
-float errSign = 0;
 float integrator = 0;
-float PI_Ctrl_Val = 0;
 float PI_Ctrl_Duty = 0.5;
 float servo_Duty = DUTY_MID;
 uint8_t dirStaus = FORWARD;
@@ -60,16 +59,16 @@
 
 void timer_Init(void){
     // 100Hz 0.01s
-    //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
+    timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
     // 50Hz 0.02s
-    timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
+    //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
     //timer_io.attach(&io_Input , 0.04f);
 }
 
 void timer_Ctrl_Interrupt(void){
     io_Input();
     servo_Ctrl_Interrupt();
-    //PI_Ctrl_Interrupt();
+    PI_Ctrl_Interrupt();
 }
 
 void servo_Ctrl_Interrupt(void){
@@ -83,9 +82,11 @@
 }
 
 void PI_Ctrl_Interrupt(void){
-    float ref_v = adc_angle.read_u16() / 4096.0f;
+    float ang_read = 0;
+    float errSign = 0;
+    float PI_Ctrl_Val = 0;
     
-    ang_read = (ref_v - 0.45f) / 0.48f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
+    ang_read = (adc_angle.read() - 0.43f) / 0.54f * 180.0f;   //0.21 ~ 0.69 respect to -90 ~ +90 degree
     cur_angle = ang_read;
     
     //////code for PI control//////
@@ -109,6 +110,7 @@
     PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
     TIM1->CCER |= 0x4;
     
+    PC.printf("%f\t%f\t%f\n",ang_cmd,cur_angle,ref_v);
 }
 
 void io_Cmd_Init(void){
@@ -117,6 +119,7 @@
 }
 
 void io_Input(void){
+    /*
     switch(dirStaus){
         case FORWARD:
         if(ang_cmd < 90){
@@ -137,11 +140,12 @@
         }
         break;
     }
-    /*
+    */
+    
     if(ang_cmd < 90){
-        ang_cmd += 15 * 0.02f;
+        ang_cmd += 15 * 0.01f;
     }
-    */
+    
 }
 
 void pwm_Init(void){