No.9 Robotics / Mbed 2 deprecated Robotics_Servo_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Robotics_Lab_Servo by LDSC_Robotics

main.cpp@7:db4ddfc0cf4e, 2016-03-10 (annotated)

Committer:
ChangYuHsuan
Date:
Thu Mar 10 11:03:39 2016 +0000
Revision:
7:db4ddfc0cf4e
Parent:
6:52f6c4d57d74 
finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:564d77fcaa70 1 /*LAB_SERVO*/
YCTung 0:564d77fcaa70 2 #include "mbed.h"
YCTung 0:564d77fcaa70 3
YCTung 0:564d77fcaa70 4 //The number will be compiled as type "double" in default
YCTung 0:564d77fcaa70 5 //Add a "f" after the number can make it compiled as type "float"
YCTung 1:3b6c9baa7d0c 6 #define Ts 0.01f //period of timer1 (s)
YCTung 2:a3c64321e9c2 7 #define Kp 0.053f
YCTung 2:a3c64321e9c2 8 #define Ki 0.013f
YCTung 0:564d77fcaa70 9
dg0704 5:d495dc55f709 10 PwmOut pwm1(D7);
dg0704 5:d495dc55f709 11 PwmOut pwm1n(D11);
YCTung 0:564d77fcaa70 12
YCTung 0:564d77fcaa70 13 AnalogIn adc(A2);//Temporary usage
YCTung 0:564d77fcaa70 14
dg0704 4:facfa2ac9a59 15 // timer variables
YCTung 0:564d77fcaa70 16 Ticker timer1;
dg0704 4:facfa2ac9a59 17 int timer1_counter;
YCTung 1:3b6c9baa7d0c 18 void timer1_interrupt(void);
YCTung 0:564d77fcaa70 19
YCTung 1:3b6c9baa7d0c 20 void init_TIMER(void);
YCTung 0:564d77fcaa70 21 void init_IO(void);
YCTung 0:564d77fcaa70 22 void init_PWM(void);
YCTung 1:3b6c9baa7d0c 23 void flash(void);
YCTung 0:564d77fcaa70 24
YCTung 1:3b6c9baa7d0c 25 //Variable(s) for PI controller
YCTung 2:a3c64321e9c2 26 float angle_ref = 0.0; //unit in degree(s), range +-90 degrees
YCTung 2:a3c64321e9c2 27 float angle_read= 0.0;
YCTung 2:a3c64321e9c2 28 float angle_check;
YCTung 0:564d77fcaa70 29 float err = 0.0;
YCTung 0:564d77fcaa70 30 float ierr = 0.0;
YCTung 0:564d77fcaa70 31 float PI_out = 0.0;
YCTung 0:564d77fcaa70 32 float pwm1_duty = 0.5;
YCTung 0:564d77fcaa70 33
YCTung 1:3b6c9baa7d0c 34 int main (void)
YCTung 1:3b6c9baa7d0c 35 {
YCTung 1:3b6c9baa7d0c 36 init_IO();
YCTung 1:3b6c9baa7d0c 37 init_PWM();
YCTung 1:3b6c9baa7d0c 38 init_TIMER();
YCTung 1:3b6c9baa7d0c 39 while(1)
YCTung 1:3b6c9baa7d0c 40 {
ChangYuHsuan 7:db4ddfc0cf4e 41 ;
YCTung 1:3b6c9baa7d0c 42 }
YCTung 1:3b6c9baa7d0c 43 }
YCTung 1:3b6c9baa7d0c 44
YCTung 0:564d77fcaa70 45 void timer1_interrupt(void)
YCTung 0:564d77fcaa70 46 {
dg0704 4:facfa2ac9a59 47 timer1_counter ++;
dg0704 4:facfa2ac9a59 48
ChangYuHsuan 7:db4ddfc0cf4e 49 if(timer1_counter == 100)
ChangYuHsuan 7:db4ddfc0cf4e 50 {
ChangYuHsuan 7:db4ddfc0cf4e 51 timer1_counter = 0;
ChangYuHsuan 7:db4ddfc0cf4e 52 angle_ref += 15.0f;
ChangYuHsuan 7:db4ddfc0cf4e 53 if (angle_ref == 45.0f)
ChangYuHsuan 7:db4ddfc0cf4e 54 break;
ChangYuHsuan 7:db4ddfc0cf4e 55 }
ChangYuHsuan 7:db4ddfc0cf4e 56
YCTung 2:a3c64321e9c2 57 angle_read = (adc.read() - 0.45f) / 0.48f * 180.0f; //0.21 ~ 0.69 respect to -90 ~ +90 degree
YCTung 2:a3c64321e9c2 58 angle_check = angle_read;
YCTung 0:564d77fcaa70 59
YCTung 1:3b6c9baa7d0c 60 //////code for PI control//////
dg0704 3:71a807b38a3e 61 err = angle_ref - angle_read;
dg0704 6:52f6c4d57d74 62 ierr += err;
ChangYuHsuan 7:db4ddfc0cf4e 63 PI_out = Kp * err + Ki * 0.01f * (ierr - err);
YCTung 2:a3c64321e9c2 64
ChangYuHsuan 7:db4ddfc0cf4e 65 // staturation
YCTung 0:564d77fcaa70 66 if(PI_out >= 0.5f)PI_out = 0.5;
YCTung 0:564d77fcaa70 67 else if(PI_out <= -0.5f)PI_out = -0.5;
YCTung 0:564d77fcaa70 68 pwm1_duty = PI_out + 0.5f;
YCTung 2:a3c64321e9c2 69 if(angle_check > 100.0f || angle_check < -100.0f)pwm1_duty = 0.5;
YCTung 0:564d77fcaa70 70 pwm1.write(pwm1_duty);
dg0704 3:71a807b38a3e 71 TIM1->CCER |= 0x4; //enable ch1 complementary output
dg0704 3:71a807b38a3e 72
YCTung 0:564d77fcaa70 73 }
YCTung 0:564d77fcaa70 74
YCTung 0:564d77fcaa70 75 void init_TIMER(void)
YCTung 0:564d77fcaa70 76 {
YCTung 0:564d77fcaa70 77 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
dg0704 4:facfa2ac9a59 78 timer1_counter = 0;
YCTung 0:564d77fcaa70 79 }
YCTung 0:564d77fcaa70 80
YCTung 0:564d77fcaa70 81 void init_IO(void)
YCTung 0:564d77fcaa70 82 {
ChangYuHsuan 7:db4ddfc0cf4e 83 angle_ref = -45.0f;
YCTung 0:564d77fcaa70 84 }
YCTung 0:564d77fcaa70 85
YCTung 0:564d77fcaa70 86 void init_PWM(void)
YCTung 0:564d77fcaa70 87 {
YCTung 0:564d77fcaa70 88 pwm1.period_us(50);
YCTung 0:564d77fcaa70 89 pwm1.write(0.5);
YCTung 0:564d77fcaa70 90 TIM1->CCER |= 0x4;
YCTung 0:564d77fcaa70 91 }