Liang HaoPo / Mbed 2 deprecated ServoMotorCtrl

servo_ctrl

Dependencies:   mbed

main.cpp@3:237f0ac0ed1b, 2016-03-10 (annotated)

Committer:
lianghaopo
Date:
Thu Mar 10 04:41:44 2016 +0000
Revision:
3:237f0ac0ed1b
Parent:
2:cca4199fa834
Child:
4:5faca3dd2acc
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 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 3:237f0ac0ed1b 15 // COMMUNICATION
lianghaopo 3:237f0ac0ed1b 16 Serial PC(USBTX,USBRX);
lianghaopo 0:0216e89de7ca 17 // PWM
lianghaopo 0:0216e89de7ca 18 PwmOut servo_Pwm(PA_0);
lianghaopo 0:0216e89de7ca 19 PwmOut PI_Ctrl_Pwm(PA_8);
lianghaopo 0:0216e89de7ca 20 PwmOut PI_Ctrl_Pwm_n(PA_7);
lianghaopo 0:0216e89de7ca 21 // ADC
lianghaopo 0:0216e89de7ca 22 AnalogIn adc_angle(PA_4);
lianghaopo 0:0216e89de7ca 23 // LED
lianghaopo 0:0216e89de7ca 24 DigitalOut user_led(LED1);
lianghaopo 0:0216e89de7ca 25 DigitalOut led2(PA_6);
lianghaopo 0:0216e89de7ca 26 // TIMER
lianghaopo 0:0216e89de7ca 27 Ticker timer_Ctrl;
lianghaopo 1:e13ff3cb7a35 28 //Ticker timer_io;
lianghaopo 0:0216e89de7ca 29 // GLOBAL
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 0:0216e89de7ca 46 int main() {
lianghaopo 0:0216e89de7ca 47 // set
lianghaopo 0:0216e89de7ca 48 io_Cmd_Init();
lianghaopo 0:0216e89de7ca 49 pwm_Init();
lianghaopo 0:0216e89de7ca 50 wait(2);
lianghaopo 0:0216e89de7ca 51 timer_Init();
lianghaopo 0:0216e89de7ca 52 // loop
lianghaopo 0:0216e89de7ca 53 while(1){
lianghaopo 0:0216e89de7ca 54 flash();
lianghaopo 0:0216e89de7ca 55 wait_ms(500);
lianghaopo 0:0216e89de7ca 56 }
lianghaopo 0:0216e89de7ca 57
lianghaopo 0:0216e89de7ca 58 }
lianghaopo 0:0216e89de7ca 59
lianghaopo 0:0216e89de7ca 60 void timer_Init(void){
lianghaopo 0:0216e89de7ca 61 // 100Hz 0.01s
lianghaopo 3:237f0ac0ed1b 62 timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 10000);
lianghaopo 0:0216e89de7ca 63 // 50Hz 0.02s
lianghaopo 3:237f0ac0ed1b 64 //timer_Ctrl.attach_us(&timer_Ctrl_Interrupt , 20000);
lianghaopo 0:0216e89de7ca 65 //timer_io.attach(&io_Input , 0.04f);
lianghaopo 0:0216e89de7ca 66 }
lianghaopo 0:0216e89de7ca 67
lianghaopo 0:0216e89de7ca 68 void timer_Ctrl_Interrupt(void){
lianghaopo 0:0216e89de7ca 69 io_Input();
lianghaopo 0:0216e89de7ca 70 servo_Ctrl_Interrupt();
lianghaopo 3:237f0ac0ed1b 71 PI_Ctrl_Interrupt();
lianghaopo 0:0216e89de7ca 72 }
lianghaopo 0:0216e89de7ca 73
lianghaopo 0:0216e89de7ca 74 void servo_Ctrl_Interrupt(void){
lianghaopo 0:0216e89de7ca 75
lianghaopo 2:cca4199fa834 76 //////code for internal control//////
lianghaopo 2:cca4199fa834 77 servo_Duty = DUTY_MID +(0.092/180) * ang_cmd;
lianghaopo 0:0216e89de7ca 78 if(servo_Duty >= 0.121f) servo_Duty = 0.121;
lianghaopo 0:0216e89de7ca 79 else if(servo_Duty <= 0.037f) servo_Duty = 0.037;
lianghaopo 0:0216e89de7ca 80 servo_Pwm.write(servo_Duty);
lianghaopo 0:0216e89de7ca 81
lianghaopo 0:0216e89de7ca 82 }
lianghaopo 0:0216e89de7ca 83
lianghaopo 0:0216e89de7ca 84 void PI_Ctrl_Interrupt(void){
lianghaopo 3:237f0ac0ed1b 85 float ang_read = 0;
lianghaopo 3:237f0ac0ed1b 86 float errSign = 0;
lianghaopo 3:237f0ac0ed1b 87 float PI_Ctrl_Val = 0;
lianghaopo 0:0216e89de7ca 88
lianghaopo 3:237f0ac0ed1b 89 ang_read = (adc_angle.read() - 0.43f) / 0.54f * 180.0f; //0.21 ~ 0.69 respect to -90 ~ +90 degree
lianghaopo 0:0216e89de7ca 90 cur_angle = ang_read;
lianghaopo 0:0216e89de7ca 91
lianghaopo 0:0216e89de7ca 92 //////code for PI control//////
lianghaopo 0:0216e89de7ca 93
lianghaopo 0:0216e89de7ca 94 errSign = ang_cmd - cur_angle;
lianghaopo 0:0216e89de7ca 95 // p_ctrl
lianghaopo 0:0216e89de7ca 96 PI_Ctrl_Val += KP * errSign;
lianghaopo 0:0216e89de7ca 97 // i_ctrl
lianghaopo 0:0216e89de7ca 98 integrator += KI * errSign * TS;
lianghaopo 0:0216e89de7ca 99 if(integrator > 0.5f) integrator = 0.5f;
lianghaopo 0:0216e89de7ca 100 else if(integrator < -0.5f) integrator = -0.5f;
lianghaopo 0:0216e89de7ca 101 //(fabs(integrator) > 0.5f)?((integrator > 0)?(integrator = 0.5f):(integrator = -0.5f)):(integrator = 1.0 * integrator);
lianghaopo 0:0216e89de7ca 102
lianghaopo 0:0216e89de7ca 103 PI_Ctrl_Val += integrator;
lianghaopo 0:0216e89de7ca 104
lianghaopo 0:0216e89de7ca 105 if(PI_Ctrl_Val > 0.5f) PI_Ctrl_Val = 0.5f;
lianghaopo 0:0216e89de7ca 106 else if(PI_Ctrl_Val < -0.5f) PI_Ctrl_Val = -0.5f;
lianghaopo 0:0216e89de7ca 107 //(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 108 PI_Ctrl_Duty = PI_Ctrl_Val + 0.5f;
lianghaopo 0:0216e89de7ca 109 if(cur_angle > 100.0f || cur_angle < -100.0f) PI_Ctrl_Duty = 0.5;
lianghaopo 0:0216e89de7ca 110 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 111 TIM1->CCER |= 0x4;
lianghaopo 0:0216e89de7ca 112
lianghaopo 3:237f0ac0ed1b 113 PC.printf("%f\t%f\t%f\n",ang_cmd,cur_angle,ref_v);
lianghaopo 0:0216e89de7ca 114 }
lianghaopo 0:0216e89de7ca 115
lianghaopo 0:0216e89de7ca 116 void io_Cmd_Init(void){
lianghaopo 0:0216e89de7ca 117 user_led = 0;
lianghaopo 0:0216e89de7ca 118 led2 = 1;
lianghaopo 0:0216e89de7ca 119 }
lianghaopo 0:0216e89de7ca 120
lianghaopo 0:0216e89de7ca 121 void io_Input(void){
lianghaopo 3:237f0ac0ed1b 122 /*
lianghaopo 2:cca4199fa834 123 switch(dirStaus){
lianghaopo 2:cca4199fa834 124 case FORWARD:
lianghaopo 2:cca4199fa834 125 if(ang_cmd < 90){
lianghaopo 2:cca4199fa834 126 ang_cmd += 15 * 0.02f;
lianghaopo 2:cca4199fa834 127 }
lianghaopo 2:cca4199fa834 128 else{
lianghaopo 2:cca4199fa834 129 ang_cmd = 90;
lianghaopo 2:cca4199fa834 130 dirStaus = INVERSE;
lianghaopo 2:cca4199fa834 131 }
lianghaopo 2:cca4199fa834 132 break;
lianghaopo 2:cca4199fa834 133 case INVERSE:
lianghaopo 2:cca4199fa834 134 if(ang_cmd > 0){
lianghaopo 2:cca4199fa834 135 ang_cmd -= 15 * 0.02f;
lianghaopo 2:cca4199fa834 136 }
lianghaopo 2:cca4199fa834 137 else{
lianghaopo 2:cca4199fa834 138 ang_cmd = 0;
lianghaopo 2:cca4199fa834 139 dirStaus = FORWARD;
lianghaopo 2:cca4199fa834 140 }
lianghaopo 2:cca4199fa834 141 break;
lianghaopo 2:cca4199fa834 142 }
lianghaopo 3:237f0ac0ed1b 143 */
lianghaopo 3:237f0ac0ed1b 144
lianghaopo 0:0216e89de7ca 145 if(ang_cmd < 90){
lianghaopo 3:237f0ac0ed1b 146 ang_cmd += 15 * 0.01f;
lianghaopo 0:0216e89de7ca 147 }
lianghaopo 3:237f0ac0ed1b 148
lianghaopo 0:0216e89de7ca 149 }
lianghaopo 0:0216e89de7ca 150
lianghaopo 0:0216e89de7ca 151 void pwm_Init(void){
lianghaopo 2:cca4199fa834 152 servo_Pwm.period_ms(20); //50Hz
lianghaopo 0:0216e89de7ca 153 servo_Pwm.write(servo_Duty);
lianghaopo 2:cca4199fa834 154 PI_Ctrl_Pwm.period_us(50); // 20000Hz
lianghaopo 0:0216e89de7ca 155 PI_Ctrl_Pwm.write(PI_Ctrl_Duty);
lianghaopo 0:0216e89de7ca 156 TIM1->CCER |= 0x4;
lianghaopo 0:0216e89de7ca 157 }
lianghaopo 0:0216e89de7ca 158
lianghaopo 0:0216e89de7ca 159 void flash(void){
lianghaopo 0:0216e89de7ca 160 user_led = !user_led;
lianghaopo 0:0216e89de7ca 161 }