Liang HaoPo / Mbed 2 deprecated ServoMotorCtrl

servo_ctrl

Dependencies:   mbed

main.cpp@2:cca4199fa834, 2016-03-09 (annotated)

Committer:
lianghaopo
Date:
Wed Mar 09 07:08:15 2016 +0000
Revision:
2:cca4199fa834
Parent:
1:e13ff3cb7a35
Child:
3:237f0ac0ed1b
version3

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