Diff: main.cpp

Revision:

0:564d77fcaa70

Child:

1:3b6c9baa7d0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 23 13:47:59 2016 +0000
@@ -0,0 +1,99 @@
+/*LAB_SERVO*/
+#include "mbed.h"
+
+//The number will be compiled as type "double" in default
+//Add a "f" after the number can make it compiled as type "float"
+#define Ts 0.01f
+#define Kp 20.0f
+#define Ki 5.0f
+
+PwmOut servo(A0);
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+
+AnalogIn adc(A2);//Temporary usage
+
+//LED1 = D13 = PA_5 (LED on Nucleo board)
+DigitalOut led1(LED1);
+DigitalOut led2(D12);
+
+Ticker timer1;
+
+void init_IO(void); 
+void init_ADC(void);
+void init_PWM(void);
+
+float ref = 0.45;       // 0.45 +(0.48/180.0)*angle, -90<angle<90
+float err = 0.0;
+float ierr = 0.0;
+float PI_out = 0.0;
+float ad_read = 0.0;    // ADC value
+float pwm1_duty = 0.5;
+float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90   
+
+void timer1_interrupt(void)
+{
+    ad_read = adc.read();
+    
+    //////code of PI control//////
+    
+    
+    
+    
+    
+    ////////////
+    if(PI_out >= 0.5f)PI_out = 0.5;
+    else if(PI_out <= -0.5f)PI_out = -0.5;
+    pwm1_duty = PI_out + 0.5f;
+    ad_read = adc.read();
+    if(ad_read > 0.69f || ad_read < 0.21f)pwm1_duty = 0.5;
+    pwm1.write(pwm1_duty);
+    TIM1->CCER |= 0x4;
+    
+    //////code of internal control//////
+    
+    
+    
+    
+    
+    ////////////
+    if(servo_duty >= 0.121f)servo_duty = 0.121;
+    else if(servo_duty <= 0.037f)servo_duty = 0.037;
+    servo.write(servo_duty);
+}
+
+void init_TIMER(void)
+{
+    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
+}
+       
+void init_IO(void)
+{
+    led1 = 0;
+    led2 = 1;
+}         
+   
+void init_PWM(void)
+{
+    servo.period_ms(20);
+    servo.write(servo_duty);
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+}
+
+void flash(void)
+{
+    led1 = !led1;
+}
+
+int main (void)
+{    
+    init_IO();
+    init_PWM();
+    init_TIMER();
+    while(1)
+    {
+        ;
+    }       
+}
\ No newline at end of file