Diff: main.cpp

Revision:

0:43250c03ce7b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 22 07:59:49 2018 +0000
@@ -0,0 +1,238 @@
+#include "mbed.h"
+#include "PID.h"
+
+PwmOut mypwm(D7);
+AnalogIn analog_value_left(A3);
+AnalogIn analog_value_right(A0);
+
+Ticker timer1;  
+Serial pc(SERIAL_TX, SERIAL_RX);
+Serial uart_1(D10, D2);
+
+int count = 1000;
+int init_PW = 1000;
+
+
+float T = 0.002;
+float meas = 0;
+float meas_old = 0;
+float meas_f = 0;
+float meas_right = 0;
+float meas_right_old = 0;
+float meas_right_f = 0;
+float meas_min = 0;
+float T_read = 0;
+float Td = 200.0;
+float err = 0;
+float err_old = 0;
+float derivative = 0;
+float dt = 0.002;
+
+float PI_out = 0;
+float PIDout = 0;
+float Kp = 1.0;
+float Kd = 1.0;
+float Scale = 0.02;
+
+int count_LR = 0;
+
+
+int display[6] = {0,0,0,0,0,0};
+char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0};      // char num[0] , num[1] : 2 start byte
+
+
+float lpf(float input, float output_old, float frequency);
+
+void init_TIMER(void);
+void timer1_interrupt(void);
+void init_uart(void);
+void separate(void);
+void uart_send(void);
+
+PID servo_pid(1.5, 1.0, 0.1, dt);
+
+int main() {
+    pc.printf("GOGOGO\n"); 
+   
+    pc.baud(9600);
+    uart_1.baud(115200);
+    mypwm.period_ms(20);
+    mypwm.pulsewidth_us( init_PW );
+    
+    init_TIMER();
+   
+    while(1) {
+
+    }
+}
+
+void init_TIMER(){
+ 
+    timer1.attach_us(&timer1_interrupt, 2000.0);   //  10ms
+}
+
+void timer1_interrupt(){
+    /*
+    if(count_LR == 0)
+    {
+        count_LR = 1;
+        
+        meas = analog_value_left.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        meas = meas * 1000; // Change the value to be in the 0 to 3300 range    
+    
+        meas_f = lpf(meas,meas_old,20);
+        meas_old = meas_f;
+        
+    
+   }
+   else 
+   {
+        count_LR = 0;
+        
+        meas_right = analog_value_right.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range    
+    
+        meas_right_f = lpf(meas_right,meas_right_old,20);
+        meas_right_old = meas_right_f;
+    }
+   */
+    meas_right = analog_value_right.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range    
+
+    meas_right_f = lpf(meas_right,meas_right_old,20);
+    meas_right_old = meas_right_f;
+   
+    
+    meas = analog_value_left.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    meas = meas * 1000; // Change the value to be in the 0 to 3300 range    
+
+    meas_f = lpf(meas,meas_old,20);
+    meas_old = meas_f;
+    
+     
+    if( meas_f < meas_right_f )
+    {
+        meas_min = meas_f;
+    }
+    else
+    {
+        meas_min = meas_right_f;
+    }   
+    
+    err = Td - meas_min;
+    derivative = ( err - err_old ) / dt;
+    err_old = err;
+
+    
+    servo_pid.Compute(Td, meas_f);
+    PIDout = servo_pid.output;
+    count = count + int(Scale * PIDout);
+    
+    
+    //count = 2000;
+    
+    
+    
+    if(count < 1000)        //1700
+    {
+        count = 1000;       //1850
+    }
+    else if(count > 1370)        //1700
+    {
+        count = 1370;       //1850
+    } 
+    
+    
+    uart_send();
+    
+/*    
+    count = count - 20;  
+    
+    
+    if(count < 1460)      
+    {
+        count = 1460;     //1850
+    }
+    else if(count > 2000)      
+    {
+        count = 2000;     //1850
+    }
+ */   
+
+    mypwm.pulsewidth_us(count);
+ printf("measure = %.0f mV  %.0f mV          count = %d \n", meas_f,meas_right_f, count);
+
+    //pc.printf("pwm set to %d %%\n", count);
+    
+    
+    
+    
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+    
+    output = (output_old + frequency*T*input) / (1 + frequency*T);
+    
+    return output;
+}
+
+int i = 0;
+void uart_send(void)
+{
+    // uart send data
+    display[0] = meas_f;
+    display[1] = meas_right_f;
+    display[2] = 100*PIDout;
+    display[3] = 100*Kp * err;
+    display[4] = 100*PI_out;
+    display[5] = count;
+    
+    //wait(0.2);
+    separate();
+
+    int j = 0;
+    int k = 1;
+    while(k) 
+    {
+        if(uart_1.writeable()) 
+        {            
+            uart_1.putc(num[i]);
+            i++;
+            j++;
+        }
+
+        if(j>1)                     // send 2 bytes
+        {
+            k = 0;
+            j = 0;
+        }
+    }
+
+    if(i>13)
+        i = 0;
+}
+
+void init_uart()
+{
+    uart_1.baud(115200);      // 設定baudrate
+}
+
+void separate(void)
+{
+    num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
+    num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
+    num[4] = display[1] >> 8;
+    num[5] = display[1] & 0x00ff;
+    num[6] = display[2] >> 8;
+    num[7] = display[2] & 0x00ff;
+    num[8] = display[3] >> 8;
+    num[9] = display[3] & 0x00ff;
+    num[10] = display[4] >> 8;
+    num[11] = display[4] & 0x00ff;
+    num[12] = display[5] >> 8;
+    num[13] = display[5] & 0x00ff;
+    
+    
+}