LDSC_Robotics_TAs / Mbed 2 deprecated Robotics_Lab_Servo

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

main.cpp@1:3b6c9baa7d0c, 2016-03-02 (annotated)

Committer:
YCTung
Date:
Wed Mar 02 08:57:00 2016 +0000
Revision:
1:3b6c9baa7d0c
Parent:
0:564d77fcaa70 
added some comment

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 1:3b6c9baa7d0c 7 #define Kp 1.0f
YCTung 1:3b6c9baa7d0c 8 #define Ki 1.0f
YCTung 0:564d77fcaa70 9
YCTung 0:564d77fcaa70 10 PwmOut servo(A0);
YCTung 0:564d77fcaa70 11 PwmOut pwm1(D7);
YCTung 0:564d77fcaa70 12 PwmOut pwm1n(D11);
YCTung 0:564d77fcaa70 13
YCTung 0:564d77fcaa70 14 AnalogIn adc(A2);//Temporary usage
YCTung 0:564d77fcaa70 15
YCTung 0:564d77fcaa70 16 //LED1 = D13 = PA_5 (LED on Nucleo board)
YCTung 0:564d77fcaa70 17 DigitalOut led1(LED1);
YCTung 0:564d77fcaa70 18 DigitalOut led2(D12);
YCTung 0:564d77fcaa70 19
YCTung 0:564d77fcaa70 20 Ticker timer1;
YCTung 1:3b6c9baa7d0c 21 void timer1_interrupt(void);
YCTung 0:564d77fcaa70 22
YCTung 1:3b6c9baa7d0c 23 void init_TIMER(void);
YCTung 0:564d77fcaa70 24 void init_IO(void);
YCTung 0:564d77fcaa70 25 void init_PWM(void);
YCTung 1:3b6c9baa7d0c 26 void flash(void);
YCTung 0:564d77fcaa70 27
YCTung 1:3b6c9baa7d0c 28 //Variable(s) for PI controller
YCTung 0:564d77fcaa70 29 float ref = 0.45; // 0.45 +(0.48/180.0)*angle, -90<angle<90
YCTung 0:564d77fcaa70 30 float err = 0.0;
YCTung 0:564d77fcaa70 31 float ierr = 0.0;
YCTung 0:564d77fcaa70 32 float PI_out = 0.0;
YCTung 0:564d77fcaa70 33 float ad_read = 0.0; // ADC value
YCTung 1:3b6c9baa7d0c 34 float ad_check;
YCTung 0:564d77fcaa70 35 float pwm1_duty = 0.5;
YCTung 1:3b6c9baa7d0c 36
YCTung 1:3b6c9baa7d0c 37 //Variable(s) for internal control
YCTung 0:564d77fcaa70 38 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
YCTung 0:564d77fcaa70 39
YCTung 1:3b6c9baa7d0c 40 int main (void)
YCTung 1:3b6c9baa7d0c 41 {
YCTung 1:3b6c9baa7d0c 42 init_IO();
YCTung 1:3b6c9baa7d0c 43 init_PWM();
YCTung 1:3b6c9baa7d0c 44 init_TIMER();
YCTung 1:3b6c9baa7d0c 45 while(1)
YCTung 1:3b6c9baa7d0c 46 {
YCTung 1:3b6c9baa7d0c 47 ;
YCTung 1:3b6c9baa7d0c 48 }
YCTung 1:3b6c9baa7d0c 49 }
YCTung 1:3b6c9baa7d0c 50
YCTung 0:564d77fcaa70 51 void timer1_interrupt(void)
YCTung 0:564d77fcaa70 52 {
YCTung 0:564d77fcaa70 53 ad_read = adc.read();
YCTung 1:3b6c9baa7d0c 54 ad_check = ad_read;
YCTung 0:564d77fcaa70 55
YCTung 1:3b6c9baa7d0c 56 //////code for PI control//////
YCTung 0:564d77fcaa70 57
YCTung 0:564d77fcaa70 58
YCTung 0:564d77fcaa70 59
YCTung 0:564d77fcaa70 60
YCTung 0:564d77fcaa70 61
YCTung 0:564d77fcaa70 62 ////////////
YCTung 0:564d77fcaa70 63 if(PI_out >= 0.5f)PI_out = 0.5;
YCTung 0:564d77fcaa70 64 else if(PI_out <= -0.5f)PI_out = -0.5;
YCTung 0:564d77fcaa70 65 pwm1_duty = PI_out + 0.5f;
YCTung 1:3b6c9baa7d0c 66 if(ad_check > 0.69f || ad_check < 0.21f)pwm1_duty = 0.5;
YCTung 0:564d77fcaa70 67 pwm1.write(pwm1_duty);
YCTung 0:564d77fcaa70 68 TIM1->CCER |= 0x4;
YCTung 0:564d77fcaa70 69
YCTung 1:3b6c9baa7d0c 70 //////code for internal control//////
YCTung 0:564d77fcaa70 71
YCTung 0:564d77fcaa70 72
YCTung 0:564d77fcaa70 73
YCTung 0:564d77fcaa70 74
YCTung 0:564d77fcaa70 75
YCTung 0:564d77fcaa70 76 ////////////
YCTung 0:564d77fcaa70 77 if(servo_duty >= 0.121f)servo_duty = 0.121;
YCTung 0:564d77fcaa70 78 else if(servo_duty <= 0.037f)servo_duty = 0.037;
YCTung 0:564d77fcaa70 79 servo.write(servo_duty);
YCTung 0:564d77fcaa70 80 }
YCTung 0:564d77fcaa70 81
YCTung 0:564d77fcaa70 82 void init_TIMER(void)
YCTung 0:564d77fcaa70 83 {
YCTung 0:564d77fcaa70 84 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
YCTung 0:564d77fcaa70 85 }
YCTung 0:564d77fcaa70 86
YCTung 0:564d77fcaa70 87 void init_IO(void)
YCTung 0:564d77fcaa70 88 {
YCTung 0:564d77fcaa70 89 led1 = 0;
YCTung 0:564d77fcaa70 90 led2 = 1;
YCTung 0:564d77fcaa70 91 }
YCTung 0:564d77fcaa70 92
YCTung 0:564d77fcaa70 93 void init_PWM(void)
YCTung 0:564d77fcaa70 94 {
YCTung 0:564d77fcaa70 95 servo.period_ms(20);
YCTung 0:564d77fcaa70 96 servo.write(servo_duty);
YCTung 0:564d77fcaa70 97 pwm1.period_us(50);
YCTung 0:564d77fcaa70 98 pwm1.write(0.5);
YCTung 0:564d77fcaa70 99 TIM1->CCER |= 0x4;
YCTung 0:564d77fcaa70 100 }
YCTung 0:564d77fcaa70 101
YCTung 0:564d77fcaa70 102 void flash(void)
YCTung 0:564d77fcaa70 103 {
YCTung 0:564d77fcaa70 104 led1 = !led1;
YCTung 0:564d77fcaa70 105 }