Liang HaoPo / Mbed 2 deprecated ServoMotorCtrl

servo_ctrl

Dependencies:   mbed

main.cpp@0:0216e89de7ca, 2016-03-08 (annotated)

Committer:
lianghaopo
Date:
Tue Mar 08 16:47:10 2016 +0000
Revision:
0:0216e89de7ca
Child:
1:e13ff3cb7a35
servo_control_version1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lianghaopo 0:0216e89de7ca 1 #include "mbed.h"
lianghaopo 0:0216e89de7ca 2
lianghaopo 0:0216e89de7ca 3 // define Constant
lianghaopo 0:0216e89de7ca 4 #define TS 0.01f
lianghaopo 0:0216e89de7ca 5 #define KP 0.053f
lianghaopo 0:0216e89de7ca 6 #define KI 0.013f
lianghaopo 0:0216e89de7ca 7 #define SERVO_MID 0.075f
lianghaopo 0:0216e89de7ca 8 #define PI_MAX 0.5f
lianghaopo 0:0216e89de7ca 9 #define I_MAX
lianghaopo 0:0216e89de7ca 10 #define SERVO_PERIOD 20
lianghaopo 0:0216e89de7ca 11
lianghaopo 0:0216e89de7ca 12 // variable declaration
lianghaopo 0:0216e89de7ca 13 // PWM
lianghaopo 0:0216e89de7ca 14 PwmOut servo_Pwm(PA_0);
lianghaopo 0:0216e89de7ca 15 PwmOut PI_Ctrl_Pwm(PA_8);
lianghaopo 0:0216e89de7ca 16 PwmOut PI_Ctrl_Pwm_n(PA_7);
lianghaopo 0:0216e89de7ca 17 // ADC
lianghaopo 0:0216e89de7ca 18 AnalogIn adc_angle(PA_4);
lianghaopo 0:0216e89de7ca 19 // LED
lianghaopo 0:0216e89de7ca 20 DigitalOut user_led(LED1);
lianghaopo 0:0216e89de7ca 21 DigitalOut led2(PA_6);
lianghaopo 0:0216e89de7ca 22 // TIMER
lianghaopo 0:0216e89de7ca 23 Ticker timer_Ctrl;
lianghaopo 0:0216e89de7ca 24 Ticker timer_io;
lianghaopo 0:0216e89de7ca 25 // GLOBAL
lianghaopo 0:0216e89de7ca 26 float ang_cmd = 0;
lianghaopo 0:0216e89de7ca 27 volatile float ang_read = 0;
lianghaopo 0:0216e89de7ca 28 volatile float cur_angle = 0;
lianghaopo 0:0216e89de7ca 29 float errSign = 0;
lianghaopo 0:0216e89de7ca 30 float integrator = 0;
lianghaopo 0:0216e89de7ca 31 float PI_Ctrl_Val = 0;
lianghaopo 0:0216e89de7ca 32 float PI_Ctrl_Duty = 0.5;
lianghaopo 0:0216e89de7ca 33 float servo_Duty = SERVO_MID;
lianghaopo 0:0216e89de7ca 34
lianghaopo 0:0216e89de7ca 35 void timer_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 36 void servo_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 37 void PI_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 38 void timer_Init(void);
lianghaopo 0:0216e89de7ca 39 void io_Cmd_Init(void);
lianghaopo 0:0216e89de7ca 40 void io_Input(void);
lianghaopo 0:0216e89de7ca 41 void pwm_Init(void);
lianghaopo 0:0216e89de7ca 42 void flash(void);
lianghaopo 0:0216e89de7ca 43
lianghaopo 0:0216e89de7ca 44 int main() {
lianghaopo 0:0216e89de7ca 45 // set
lianghaopo 0:0216e89de7ca 46 io_Cmd_Init();
lianghaopo 0:0216e89de7ca 47 pwm_Init();
lianghaopo 0:0216e89de7ca 48 wait(2);
lianghaopo 0:0216e89de7ca 49 timer_Init();
lianghaopo 0:0216e89de7ca 50 // loop
lianghaopo 0:0216e89de7ca 51 while(1){
lianghaopo 0:0216e89de7ca 52 flash();
lianghaopo 0:0216e89de7ca 53 wait_ms(500);
lianghaopo 0:0216e89de7ca 54 }
lianghaopo 0:0216e89de7ca 55
lianghaopo 0:0216e89de7ca 56 }
lianghaopo 0:0216e89de7ca 57
lianghaopo 0:0216e89de7ca 58 void timer_Init(void){
lianghaopo 0:0216e89de7ca 59 // 100Hz 0.01s
lianghaopo 0:0216e89de7ca 60 //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
lianghaopo 0:0216e89de7ca 61 // 50Hz 0.02s
lianghaopo 0:0216e89de7ca 62 timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
lianghaopo 0:0216e89de7ca 63 //timer_io.attach(&io_Input , 0.04f);
lianghaopo 0:0216e89de7ca 64 }
lianghaopo 0:0216e89de7ca 65
lianghaopo 0:0216e89de7ca 66 void timer_Ctrl_Interrupt(void){
lianghaopo 0:0216e89de7ca 67 io_Input();
lianghaopo 0:0216e89de7ca 68 servo_Ctrl_Interrupt();
lianghaopo 0:0216e89de7ca 69 //PI_Ctrl_Interrupt();
lianghaopo 0:0216e89de7ca 70 }
lianghaopo 0:0216e89de7ca 71
lianghaopo 0:0216e89de7ca 72 void servo_Ctrl_Interrupt(void){
lianghaopo 0:0216e89de7ca 73
lianghaopo 0:0216e89de7ca 74 servo_Duty = 0.079 +(0.084/180) * ang_cmd;
lianghaopo 0:0216e89de7ca 75 if(servo_Duty >= 0.121f) servo_Duty = 0.121;
lianghaopo 0:0216e89de7ca 76 else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
lianghaopo 0:0216e89de7ca 77 servo_Pwm.write(servo_Duty);
lianghaopo 0:0216e89de7ca 78
lianghaopo 0:0216e89de7ca 79 }
lianghaopo 0:0216e89de7ca 80
lianghaopo 0:0216e89de7ca 81 void PI_Ctrl_Interrupt(void){
lianghaopo 0:0216e89de7ca 82 float ref_v = adc_angle.read_u16() / 4096.0f;
lianghaopo 0:0216e89de7ca 83
lianghaopo 0:0216e89de7ca 84 ang_read = (ref_v - 0.45f) / 0.48f * 180.0f; //0.21 ~ 0.69 respect to -90 ~ +90 degree
lianghaopo 0:0216e89de7ca 85 cur_angle = ang_read;
lianghaopo 0:0216e89de7ca 86
lianghaopo 0:0216e89de7ca 87 //////code for PI control//////
lianghaopo 0:0216e89de7ca 88
lianghaopo 0:0216e89de7ca 89 errSign = ang_cmd - cur_angle;
lianghaopo 0:0216e89de7ca 90 // p_ctrl
lianghaopo 0:0216e89de7ca 91 PI_Ctrl_Val += KP * errSign;
lianghaopo 0:0216e89de7ca 92 // i_ctrl
lianghaopo 0:0216e89de7ca 93 integrator += KI * errSign * TS;
lianghaopo 0:0216e89de7ca 94 if(integrator > 0.5f) integrator = 0.5f;
lianghaopo 0:0216e89de7ca 95 else if(integrator < -0.5f) integrator = -0.5f;
lianghaopo 0:0216e89de7ca 96 //(fabs(integrator) > 0.5f)?((integrator > 0)?(integrator = 0.5f):(integrator = -0.5f)):(integrator = 1.0 * integrator);
lianghaopo 0:0216e89de7ca 97
lianghaopo 0:0216e89de7ca 98 PI_Ctrl_Val += integrator;
lianghaopo 0:0216e89de7ca 99
lianghaopo 0:0216e89de7ca 100 if(PI_Ctrl_Val > 0.5f) PI_Ctrl_Val = 0.5f;
lianghaopo 0:0216e89de7ca 101 else if(PI_Ctrl_Val < -0.5f) PI_Ctrl_Val = -0.5f;
lianghaopo 0:0216e89de7ca 102 //(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);
lianghaopo 0:0216e89de7ca 103 PI_Ctrl_Duty = PI_Ctrl_Val + 0.5f;
lianghaopo 0:0216e89de7ca 104 if(cur_angle > 100.0f || cur_angle < -100.0f) PI_Ctrl_Duty = 0.5;
lianghaopo 0:0216e89de7ca 105 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 106 TIM1->CCER |= 0x4;
lianghaopo 0:0216e89de7ca 107
lianghaopo 0:0216e89de7ca 108 //////code for internal control//////
lianghaopo 0:0216e89de7ca 109
lianghaopo 0:0216e89de7ca 110 servo_Duty = 0.079 +(0.084/180) * ang_cmd;
lianghaopo 0:0216e89de7ca 111
lianghaopo 0:0216e89de7ca 112 if(servo_Duty >= 0.121f) servo_Duty = 0.121;
lianghaopo 0:0216e89de7ca 113 else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
lianghaopo 0:0216e89de7ca 114 servo_Pwm.write(servo_Duty);
lianghaopo 0:0216e89de7ca 115
lianghaopo 0:0216e89de7ca 116 }
lianghaopo 0:0216e89de7ca 117
lianghaopo 0:0216e89de7ca 118 void io_Cmd_Init(void){
lianghaopo 0:0216e89de7ca 119 user_led = 0;
lianghaopo 0:0216e89de7ca 120 led2 = 1;
lianghaopo 0:0216e89de7ca 121 }
lianghaopo 0:0216e89de7ca 122
lianghaopo 0:0216e89de7ca 123 void io_Input(void){
lianghaopo 0:0216e89de7ca 124 if(ang_cmd < 90){
lianghaopo 0:0216e89de7ca 125 ang_cmd += 15 * 0.02f;
lianghaopo 0:0216e89de7ca 126 }
lianghaopo 0:0216e89de7ca 127 }
lianghaopo 0:0216e89de7ca 128
lianghaopo 0:0216e89de7ca 129 void pwm_Init(void){
lianghaopo 0:0216e89de7ca 130 servo_Pwm.period_ms(20);//50Hz
lianghaopo 0:0216e89de7ca 131 servo_Pwm.write(servo_Duty);
lianghaopo 0:0216e89de7ca 132 PI_Ctrl_Pwm.period_us(50);
lianghaopo 0:0216e89de7ca 133 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 134 TIM1->CCER |= 0x4;
lianghaopo 0:0216e89de7ca 135 }
lianghaopo 0:0216e89de7ca 136
lianghaopo 0:0216e89de7ca 137 void flash(void){
lianghaopo 0:0216e89de7ca 138 user_led = !user_led;
lianghaopo 0:0216e89de7ca 139 }