Diff: main.cpp

Revision:

2:cca4199fa834

Parent:

1:e13ff3cb7a35

Child:

3:237f0ac0ed1b

--- a/main.cpp	Tue Mar 08 17:30:58 2016 +0000
+++ b/main.cpp	Wed Mar 09 07:08:15 2016 +0000
@@ -4,10 +4,12 @@
 #define TS 0.01f
 #define KP 0.053f
 #define KI 0.013f
-#define SERVO_MID 0.075f
+#define DUTY_MID 0.075f
 #define PI_MAX 0.5f
 #define I_MAX 
 #define SERVO_PERIOD 20
+#define FORWARD 0
+#define INVERSE 1
 
 // variable declaration
 // PWM
@@ -30,8 +32,9 @@
 float integrator = 0;
 float PI_Ctrl_Val = 0;
 float PI_Ctrl_Duty = 0.5;
-float servo_Duty = SERVO_MID;
-
+float servo_Duty = DUTY_MID;
+uint8_t dirStaus = FORWARD;
+// function definition
 void timer_Ctrl_Interrupt(void);
 void servo_Ctrl_Interrupt(void);
 void PI_Ctrl_Interrupt(void);
@@ -71,7 +74,8 @@
 
 void servo_Ctrl_Interrupt(void){
     
-    servo_Duty = 0.079 +(0.084/180) * ang_cmd;
+    //////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);
@@ -105,14 +109,6 @@
     PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
     TIM1->CCER |= 0x4;
     
-    //////code for internal control//////
-    
-    servo_Duty = 0.079 +(0.084/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 io_Cmd_Init(void){
@@ -121,15 +117,37 @@
 }
 
 void io_Input(void){
+    switch(dirStaus){
+        case FORWARD:
+        if(ang_cmd < 90){
+            ang_cmd += 15 * 0.02f;
+        }
+        else{
+            ang_cmd = 90;
+            dirStaus = INVERSE;
+        }
+        break;
+        case INVERSE:
+        if(ang_cmd > 0){
+            ang_cmd -= 15 * 0.02f;
+        }
+        else{
+            ang_cmd = 0;
+            dirStaus = FORWARD;
+        }
+        break;
+    }
+    /*
     if(ang_cmd < 90){
         ang_cmd += 15 * 0.02f;
     }
+    */
 }
 
 void pwm_Init(void){
-    servo_Pwm.period_ms(20);//50Hz
+    servo_Pwm.period_ms(20); //50Hz
     servo_Pwm.write(servo_Duty);
-    PI_Ctrl_Pwm.period_us(50);
+    PI_Ctrl_Pwm.period_us(50); // 20000Hz
     PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
     TIM1->CCER |= 0x4;
 }