Diff: main.cpp

Revision:

4:5faca3dd2acc

Parent:

3:237f0ac0ed1b

--- a/main.cpp	Thu Mar 10 04:41:44 2016 +0000
+++ b/main.cpp	Thu Mar 10 11:26:07 2016 +0000
@@ -6,7 +6,7 @@
 #define KI 0.013f
 #define DUTY_MID 0.075f
 #define PI_MAX 0.5f
-#define I_MAX 
+#define I_MAX 0.5f
 #define SERVO_PERIOD 20
 #define FORWARD 0
 #define INVERSE 1
@@ -15,18 +15,18 @@
 // COMMUNICATION
 Serial PC(USBTX,USBRX);
 // PWM
-PwmOut servo_Pwm(PA_0);
-PwmOut PI_Ctrl_Pwm(PA_8);
-PwmOut PI_Ctrl_Pwm_n(PA_7);
+PwmOut servo_Pwm(PA_0);// pin A0
+PwmOut PI_Ctrl_Pwm(PA_8); // pin D7
+PwmOut PI_Ctrl_Pwm_n(PA_7); // pin D11
 // ADC
-AnalogIn adc_angle(PA_4);
+AnalogIn adc_angle(PA_4); // pin A2
 // LED
-DigitalOut user_led(LED1);
-DigitalOut led2(PA_6);
+DigitalOut user_led(LED1); // pin D13
+DigitalOut led2(PA_6); // pin D12
 // TIMER
 Ticker timer_Ctrl;
-//Ticker timer_io;
 // GLOBAL
+bool io_staus = false;
 volatile float ang_cmd = 0;
 volatile float cur_angle = 0;
 float integrator = 0;
@@ -43,54 +43,65 @@
 void pwm_Init(void);
 void flash(void);
 
-int main() {
+int main()
+{
     // set
     io_Cmd_Init();
     pwm_Init();
+    timer_Init();
     wait(2);
-    timer_Init();  
+    io_staus = true;
     // loop
-    while(1){
+    while(1) {
         flash();
-        wait_ms(500);
+        wait_ms(500); // 2Hz flash
     }
-    
+
 }
 
-void timer_Init(void){
+void timer_Init(void)
+{
     // 100Hz 0.01s
     timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
     // 50Hz 0.02s
     //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
-    //timer_io.attach(&io_Input , 0.04f);
 }
 
-void timer_Ctrl_Interrupt(void){
-    io_Input();
+void timer_Ctrl_Interrupt(void)
+{
+    // angle cmd
+    if(io_staus)    
+        io_Input();
+    else    
+        ang_cmd = 0;
+    // servo internal control
     servo_Ctrl_Interrupt();
+    // servo PI control
     PI_Ctrl_Interrupt();
 }
 
-void servo_Ctrl_Interrupt(void){
-    
+void servo_Ctrl_Interrupt(void)
+{
+
     //////code for internal control//////
     servo_Duty = DUTY_MID +(0.092/180) * ang_cmd;
     if(servo_Duty >= 0.121f) servo_Duty = 0.121;
     else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
     servo_Pwm.write(servo_Duty);
-    
+
 }
 
-void PI_Ctrl_Interrupt(void){
+void PI_Ctrl_Interrupt(void)
+{
+    
     float ang_read = 0;
     float errSign = 0;
     float PI_Ctrl_Val = 0;
     
+    //////code for PI control//////
     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//////
-    
+
     errSign = ang_cmd - cur_angle;
     // p_ctrl
     PI_Ctrl_Val += KP *  errSign;
@@ -99,26 +110,38 @@
     if(integrator > 0.5f)   integrator = 0.5f;
     else if(integrator < -0.5f) integrator = -0.5f;
     //(fabs(integrator) > 0.5f)?((integrator > 0)?(integrator = 0.5f):(integrator = -0.5f)):(integrator = 1.0 * integrator);
-    
+
     PI_Ctrl_Val += integrator;
-    
+
     if(PI_Ctrl_Val > 0.5f)  PI_Ctrl_Val = 0.5f;
     else if(PI_Ctrl_Val < -0.5f) PI_Ctrl_Val = -0.5f;
     //(fabs(PI_Ctrl_Val) > 0.5f)?((PI_Ctrl_Val > 0)?(PI_Ctrl_Val = 0.5f):(PI_Ctrl_Val = -0.5f)):(PI_Ctrl_Val = 1.0 * PI_Ctrl_Val);
     PI_Ctrl_Duty = PI_Ctrl_Val + 0.5f;
-    if(cur_angle > 100.0f || cur_angle < -100.0f)   PI_Ctrl_Duty = 0.5;
+    if(cur_angle > 100.0f) {
+        PI_Ctrl_Duty = 0.4;
+        PI_Ctrl_Val = 0.0;
+    } else if(cur_angle < -100.0f) {
+        PI_Ctrl_Duty = 0.6;
+        PI_Ctrl_Val = 0.0;
+    }
     PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
     TIM1->CCER |= 0x4;
+
+    PC.printf("%f %f %f %f\n",ang_cmd,cur_angle,PI_Ctrl_Val,PI_Ctrl_Duty);
     
-    PC.printf("%f\t%f\t%f\n",ang_cmd,cur_angle,ref_v);
+    //PI_Ctrl_Pwm.write(0.8);
+    //TIM1->CCER |= 0x4;
+
 }
 
-void io_Cmd_Init(void){
+void io_Cmd_Init(void)
+{
     user_led = 0;
     led2 = 1;
 }
 
-void io_Input(void){
+void io_Input(void)
+{
     /*
     switch(dirStaus){
         case FORWARD:
@@ -141,14 +164,15 @@
         break;
     }
     */
-    
-    if(ang_cmd < 90){
+
+    if(ang_cmd < 90) {
         ang_cmd += 15 * 0.01f;
     }
-    
+
 }
 
-void pwm_Init(void){
+void pwm_Init(void)
+{
     servo_Pwm.period_ms(20); //50Hz
     servo_Pwm.write(servo_Duty);
     PI_Ctrl_Pwm.period_us(50); // 20000Hz
@@ -156,6 +180,7 @@
     TIM1->CCER |= 0x4;
 }
 
-void flash(void){
+void flash(void)
+{
     user_led = !user_led;
 }