Revision 0:0216e89de7ca, committed 2016-03-08

Comitter:

lianghaopo

Date:

Tue Mar 08 16:47:10 2016 +0000

Child:

1:e13ff3cb7a35

Commit message:

servo_control_version1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 08 16:47:10 2016 +0000
@@ -0,0 +1,139 @@
+#include "mbed.h"
+
+// define Constant
+#define TS 0.01f
+#define KP 0.053f
+#define KI 0.013f
+#define SERVO_MID 0.075f
+#define PI_MAX 0.5f
+#define I_MAX 
+#define SERVO_PERIOD 20
+
+// variable declaration
+// PWM
+PwmOut servo_Pwm(PA_0);
+PwmOut PI_Ctrl_Pwm(PA_8);
+PwmOut PI_Ctrl_Pwm_n(PA_7);
+// ADC
+AnalogIn adc_angle(PA_4);
+// LED
+DigitalOut user_led(LED1);
+DigitalOut led2(PA_6);
+// TIMER
+Ticker timer_Ctrl;
+Ticker timer_io;
+// GLOBAL
+float ang_cmd = 0;
+volatile float ang_read = 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 = SERVO_MID;
+
+void timer_Ctrl_Interrupt(void);
+void servo_Ctrl_Interrupt(void);
+void PI_Ctrl_Interrupt(void);
+void timer_Init(void);
+void io_Cmd_Init(void);
+void io_Input(void);
+void pwm_Init(void);
+void flash(void);
+
+int main() {
+    // set
+    io_Cmd_Init();
+    pwm_Init();
+    wait(2);
+    timer_Init();  
+    // loop
+    while(1){
+        flash();
+        wait_ms(500);
+    }
+    
+}
+
+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();
+    servo_Ctrl_Interrupt();
+    //PI_Ctrl_Interrupt();
+}
+
+void servo_Ctrl_Interrupt(void){
+    
+    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 PI_Ctrl_Interrupt(void){
+    float ref_v = adc_angle.read_u16() / 4096.0f;
+    
+    ang_read = (ref_v - 0.45f) / 0.48f * 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;
+    // i_ctrl
+    integrator += KI * errSign * TS;
+    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;
+    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){
+    user_led = 0;
+    led2 = 1;
+}
+
+void io_Input(void){
+    if(ang_cmd < 90){
+        ang_cmd += 15 * 0.02f;
+    }
+}
+
+void pwm_Init(void){
+    servo_Pwm.period_ms(20);//50Hz
+    servo_Pwm.write(servo_Duty);
+    PI_Ctrl_Pwm.period_us(50);
+    PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
+    TIM1->CCER |= 0x4;
+}
+
+void flash(void){
+    user_led = !user_led;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 08 16:47:10 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb
\ No newline at end of file