Liang HaoPo / Mbed 2 deprecated ServoMotorCtrl

servo_ctrl

Dependencies:   mbed

main.cpp@4:5faca3dd2acc, 2016-03-10 (annotated)

Committer:
lianghaopo
Date:
Thu Mar 10 11:26:07 2016 +0000
Revision:
4:5faca3dd2acc
Parent:
3:237f0ac0ed1b 
new_version

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 4:5faca3dd2acc 9 #define I_MAX 0.5f
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 3:237f0ac0ed1b 15 // COMMUNICATION
lianghaopo 3:237f0ac0ed1b 16 Serial PC(USBTX,USBRX);
lianghaopo 0:0216e89de7ca 17 // PWM
lianghaopo 4:5faca3dd2acc 18 PwmOut servo_Pwm(PA_0);// pin A0
lianghaopo 4:5faca3dd2acc 19 PwmOut PI_Ctrl_Pwm(PA_8); // pin D7
lianghaopo 4:5faca3dd2acc 20 PwmOut PI_Ctrl_Pwm_n(PA_7); // pin D11
lianghaopo 0:0216e89de7ca 21 // ADC
lianghaopo 4:5faca3dd2acc 22 AnalogIn adc_angle(PA_4); // pin A2
lianghaopo 0:0216e89de7ca 23 // LED
lianghaopo 4:5faca3dd2acc 24 DigitalOut user_led(LED1); // pin D13
lianghaopo 4:5faca3dd2acc 25 DigitalOut led2(PA_6); // pin D12
lianghaopo 0:0216e89de7ca 26 // TIMER
lianghaopo 0:0216e89de7ca 27 Ticker timer_Ctrl;
lianghaopo 0:0216e89de7ca 28 // GLOBAL
lianghaopo 4:5faca3dd2acc 29 bool io_staus = false;
lianghaopo 3:237f0ac0ed1b 30 volatile float ang_cmd = 0;
lianghaopo 0:0216e89de7ca 31 volatile float cur_angle = 0;
lianghaopo 0:0216e89de7ca 32 float integrator = 0;
lianghaopo 0:0216e89de7ca 33 float PI_Ctrl_Duty = 0.5;
lianghaopo 2:cca4199fa834 34 float servo_Duty = DUTY_MID;
lianghaopo 2:cca4199fa834 35 uint8_t dirStaus = FORWARD;
lianghaopo 2:cca4199fa834 36 // function definition
lianghaopo 0:0216e89de7ca 37 void timer_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 38 void servo_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 39 void PI_Ctrl_Interrupt(void);
lianghaopo 0:0216e89de7ca 40 void timer_Init(void);
lianghaopo 0:0216e89de7ca 41 void io_Cmd_Init(void);
lianghaopo 0:0216e89de7ca 42 void io_Input(void);
lianghaopo 0:0216e89de7ca 43 void pwm_Init(void);
lianghaopo 0:0216e89de7ca 44 void flash(void);
lianghaopo 0:0216e89de7ca 45
lianghaopo 4:5faca3dd2acc 46 int main()
lianghaopo 4:5faca3dd2acc 47 {
lianghaopo 0:0216e89de7ca 48 // set
lianghaopo 0:0216e89de7ca 49 io_Cmd_Init();
lianghaopo 0:0216e89de7ca 50 pwm_Init();
lianghaopo 4:5faca3dd2acc 51 timer_Init();
lianghaopo 0:0216e89de7ca 52 wait(2);
lianghaopo 4:5faca3dd2acc 53 io_staus = true;
lianghaopo 0:0216e89de7ca 54 // loop
lianghaopo 4:5faca3dd2acc 55 while(1) {
lianghaopo 0:0216e89de7ca 56 flash();
lianghaopo 4:5faca3dd2acc 57 wait_ms(500); // 2Hz flash
lianghaopo 0:0216e89de7ca 58 }
lianghaopo 4:5faca3dd2acc 59
lianghaopo 0:0216e89de7ca 60 }
lianghaopo 0:0216e89de7ca 61
lianghaopo 4:5faca3dd2acc 62 void timer_Init(void)
lianghaopo 4:5faca3dd2acc 63 {
lianghaopo 0:0216e89de7ca 64 // 100Hz 0.01s
lianghaopo 3:237f0ac0ed1b 65 timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
lianghaopo 0:0216e89de7ca 66 // 50Hz 0.02s
lianghaopo 3:237f0ac0ed1b 67 //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
lianghaopo 0:0216e89de7ca 68 }
lianghaopo 0:0216e89de7ca 69
lianghaopo 4:5faca3dd2acc 70 void timer_Ctrl_Interrupt(void)
lianghaopo 4:5faca3dd2acc 71 {
lianghaopo 4:5faca3dd2acc 72 // angle cmd
lianghaopo 4:5faca3dd2acc 73 if(io_staus)
lianghaopo 4:5faca3dd2acc 74 io_Input();
lianghaopo 4:5faca3dd2acc 75 else
lianghaopo 4:5faca3dd2acc 76 ang_cmd = 0;
lianghaopo 4:5faca3dd2acc 77 // servo internal control
lianghaopo 0:0216e89de7ca 78 servo_Ctrl_Interrupt();
lianghaopo 4:5faca3dd2acc 79 // servo PI control
lianghaopo 3:237f0ac0ed1b 80 PI_Ctrl_Interrupt();
lianghaopo 0:0216e89de7ca 81 }
lianghaopo 0:0216e89de7ca 82
lianghaopo 4:5faca3dd2acc 83 void servo_Ctrl_Interrupt(void)
lianghaopo 4:5faca3dd2acc 84 {
lianghaopo 4:5faca3dd2acc 85
lianghaopo 2:cca4199fa834 86 //////code for internal control//////
lianghaopo 2:cca4199fa834 87 servo_Duty = DUTY_MID +(0.092/180) * ang_cmd;
lianghaopo 0:0216e89de7ca 88 if(servo_Duty >= 0.121f) servo_Duty = 0.121;
lianghaopo 0:0216e89de7ca 89 else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
lianghaopo 0:0216e89de7ca 90 servo_Pwm.write(servo_Duty);
lianghaopo 4:5faca3dd2acc 91
lianghaopo 0:0216e89de7ca 92 }
lianghaopo 0:0216e89de7ca 93
lianghaopo 4:5faca3dd2acc 94 void PI_Ctrl_Interrupt(void)
lianghaopo 4:5faca3dd2acc 95 {
lianghaopo 4:5faca3dd2acc 96
lianghaopo 3:237f0ac0ed1b 97 float ang_read = 0;
lianghaopo 3:237f0ac0ed1b 98 float errSign = 0;
lianghaopo 3:237f0ac0ed1b 99 float PI_Ctrl_Val = 0;
lianghaopo 0:0216e89de7ca 100
lianghaopo 4:5faca3dd2acc 101 //////code for PI control//////
lianghaopo 3:237f0ac0ed1b 102 ang_read = (adc_angle.read() - 0.43f) / 0.54f * 180.0f; //0.21 ~ 0.69 respect to -90 ~ +90 degree
lianghaopo 0:0216e89de7ca 103 cur_angle = ang_read;
lianghaopo 4:5faca3dd2acc 104
lianghaopo 0:0216e89de7ca 105 errSign = ang_cmd - cur_angle;
lianghaopo 0:0216e89de7ca 106 // p_ctrl
lianghaopo 0:0216e89de7ca 107 PI_Ctrl_Val += KP * errSign;
lianghaopo 0:0216e89de7ca 108 // i_ctrl
lianghaopo 0:0216e89de7ca 109 integrator += KI * errSign * TS;
lianghaopo 0:0216e89de7ca 110 if(integrator > 0.5f) integrator = 0.5f;
lianghaopo 0:0216e89de7ca 111 else if(integrator < -0.5f) integrator = -0.5f;
lianghaopo 0:0216e89de7ca 112 //(fabs(integrator) > 0.5f)?((integrator > 0)?(integrator = 0.5f):(integrator = -0.5f)):(integrator = 1.0 * integrator);
lianghaopo 4:5faca3dd2acc 113
lianghaopo 0:0216e89de7ca 114 PI_Ctrl_Val += integrator;
lianghaopo 4:5faca3dd2acc 115
lianghaopo 0:0216e89de7ca 116 if(PI_Ctrl_Val > 0.5f) PI_Ctrl_Val = 0.5f;
lianghaopo 0:0216e89de7ca 117 else if(PI_Ctrl_Val < -0.5f) PI_Ctrl_Val = -0.5f;
lianghaopo 0:0216e89de7ca 118 //(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 119 PI_Ctrl_Duty = PI_Ctrl_Val + 0.5f;
lianghaopo 4:5faca3dd2acc 120 if(cur_angle > 100.0f) {
lianghaopo 4:5faca3dd2acc 121 PI_Ctrl_Duty = 0.4;
lianghaopo 4:5faca3dd2acc 122 PI_Ctrl_Val = 0.0;
lianghaopo 4:5faca3dd2acc 123 } else if(cur_angle < -100.0f) {
lianghaopo 4:5faca3dd2acc 124 PI_Ctrl_Duty = 0.6;
lianghaopo 4:5faca3dd2acc 125 PI_Ctrl_Val = 0.0;
lianghaopo 4:5faca3dd2acc 126 }
lianghaopo 0:0216e89de7ca 127 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 128 TIM1->CCER |= 0x4;
lianghaopo 4:5faca3dd2acc 129
lianghaopo 4:5faca3dd2acc 130 PC.printf("%f %f %f %f\n",ang_cmd,cur_angle,PI_Ctrl_Val,PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 131
lianghaopo 4:5faca3dd2acc 132 //PI_Ctrl_Pwm.write(0.8);
lianghaopo 4:5faca3dd2acc 133 //TIM1->CCER |= 0x4;
lianghaopo 4:5faca3dd2acc 134
lianghaopo 0:0216e89de7ca 135 }
lianghaopo 0:0216e89de7ca 136
lianghaopo 4:5faca3dd2acc 137 void io_Cmd_Init(void)
lianghaopo 4:5faca3dd2acc 138 {
lianghaopo 0:0216e89de7ca 139 user_led = 0;
lianghaopo 0:0216e89de7ca 140 led2 = 1;
lianghaopo 0:0216e89de7ca 141 }
lianghaopo 0:0216e89de7ca 142
lianghaopo 4:5faca3dd2acc 143 void io_Input(void)
lianghaopo 4:5faca3dd2acc 144 {
lianghaopo 3:237f0ac0ed1b 145 /*
lianghaopo 2:cca4199fa834 146 switch(dirStaus){
lianghaopo 2:cca4199fa834 147 case FORWARD:
lianghaopo 2:cca4199fa834 148 if(ang_cmd < 90){
lianghaopo 2:cca4199fa834 149 ang_cmd += 15 * 0.02f;
lianghaopo 2:cca4199fa834 150 }
lianghaopo 2:cca4199fa834 151 else{
lianghaopo 2:cca4199fa834 152 ang_cmd = 90;
lianghaopo 2:cca4199fa834 153 dirStaus = INVERSE;
lianghaopo 2:cca4199fa834 154 }
lianghaopo 2:cca4199fa834 155 break;
lianghaopo 2:cca4199fa834 156 case INVERSE:
lianghaopo 2:cca4199fa834 157 if(ang_cmd > 0){
lianghaopo 2:cca4199fa834 158 ang_cmd -= 15 * 0.02f;
lianghaopo 2:cca4199fa834 159 }
lianghaopo 2:cca4199fa834 160 else{
lianghaopo 2:cca4199fa834 161 ang_cmd = 0;
lianghaopo 2:cca4199fa834 162 dirStaus = FORWARD;
lianghaopo 2:cca4199fa834 163 }
lianghaopo 2:cca4199fa834 164 break;
lianghaopo 2:cca4199fa834 165 }
lianghaopo 3:237f0ac0ed1b 166 */
lianghaopo 4:5faca3dd2acc 167
lianghaopo 4:5faca3dd2acc 168 if(ang_cmd < 90) {
lianghaopo 3:237f0ac0ed1b 169 ang_cmd += 15 * 0.01f;
lianghaopo 0:0216e89de7ca 170 }
lianghaopo 4:5faca3dd2acc 171
lianghaopo 0:0216e89de7ca 172 }
lianghaopo 0:0216e89de7ca 173
lianghaopo 4:5faca3dd2acc 174 void pwm_Init(void)
lianghaopo 4:5faca3dd2acc 175 {
lianghaopo 2:cca4199fa834 176 servo_Pwm.period_ms(20); //50Hz
lianghaopo 0:0216e89de7ca 177 servo_Pwm.write(servo_Duty);
lianghaopo 2:cca4199fa834 178 PI_Ctrl_Pwm.period_us(50); // 20000Hz
lianghaopo 0:0216e89de7ca 179 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 180 TIM1->CCER |= 0x4;
lianghaopo 0:0216e89de7ca 181 }
lianghaopo 0:0216e89de7ca 182
lianghaopo 4:5faca3dd2acc 183 void flash(void)
lianghaopo 4:5faca3dd2acc 184 {
lianghaopo 0:0216e89de7ca 185 user_led = !user_led;
lianghaopo 0:0216e89de7ca 186 }