Revision 0:72e0bf0dbe7c, committed 2014-11-06

Comitter:

higedura

Date:

Thu Nov 06 10:13:27 2014 +0000

Commit message:

Velocity feedback for YAL RChack

Changed in this revision

diff -r 000000000000 -r 72e0bf0dbe7c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 06 10:13:27 2014 +0000
@@ -0,0 +1,287 @@
+#include "mbed.h"
+
+#define N_CHAN      3   // the numbers of channels
+#define SIZE_DATA   16  // size of vz data
+
+Serial pc(USBTX, USBRX);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//InterruptIn ch1_in(p5);     // roll
+InterruptIn ch2_in(p5);     // pitch
+InterruptIn ch3_in(p6);     // throttle
+//InterruptIn ch4_in(p7);     // taw
+InterruptIn ch5_in(p7);     // aux1
+//InterruptIn ch6_in(p18);    // aux2
+
+DigitalIn button(p12);
+
+Serial xbee(p13, p14);   // tx, rx
+
+//PwmOut ch1_out(p21);        // rall
+PwmOut ch2_out(p21);        // pitch
+PwmOut ch3_out(p22);        // throttle
+//PwmOut ch4_out(p24);        // yaw
+//PwmOut ch5_out(p25);        // aux1
+//PwmOut ch6_out(p26);        // aux2
+
+///////////////////// Function prototype ////////////////////
+//void roll_up(void);
+void pitch_up(void);
+void thr_up(void);
+//void yaw_up(void);
+void aux1_up(void);
+//void aux2_up(void);
+
+//void rall_down(void);
+void pitch_down(void);
+void thr_down(void);
+//void yaw_down(void);
+void aux1_down(void);
+//void aux2_down(void);
+
+///////////////////// Timers ////////////////////
+//Timer t_roll;
+Timer t_pitch;
+Timer t_thr;
+//Timer t_yaw;
+Timer t_aux1;
+//Timer t_aux2;
+
+///////////////////// Global variable ////////////////////
+int g_pwm_pulse[N_CHAN]      =   {0};    // pitch thr aux1
+
+///////////////////// Local file system ////////////////////
+LocalFileSystem local("local"); 
+
+///////////////////// Main loop ////////////////////
+int main() {
+    
+    pc.baud(921600);
+    xbee.baud(57600);
+    
+    double GAIN_VEL_X   =   0.2;
+    //double GAIN_VEL_Z   =   0;
+    
+    double VEL_X_TARGET =   500;
+    
+    double LIMIT_PULTH_WIDTH_CH2_OUT_LOW    =   0.0013;
+    double LIMIT_PULTH_WIDTH_CH2_OUT_UP     =   0.0017;
+    
+    int pos[2]   =   {0};
+    int vel[2]   =   {0};
+    
+    int PULTH_WIDTH_RC_TRIM_CH2 =   0;
+    int PULTH_WIDTH_RC_TRIM_CH3 =   0;
+    
+    double pulse_width_ch2_out  =   0.0015;
+    double pulse_width_ch3_out  =   0.001;
+    
+    ch2_out.period(0.02);
+    ch3_out.period(0.02);
+
+    ch2_out.pulsewidth(0.0015); // pitch
+    ch3_out.pulsewidth(0.0011); // thr
+    
+    /////////////////// Interrupt for xbee ////////////////////
+    //xbee.attach(&get_vel);
+    
+    //////////////////// Interrupt for RC reciever ////////////////////
+    //ch1_in.rise(&roll_up);      ch1_in.fall(&roll_down);
+    ch2_in.rise(&pitch_up);    ch2_in.fall(&pitch_down);
+    ch3_in.rise(&thr_up);      ch3_in.fall(&thr_down);
+    //ch4_in.rise(&yaw_up);     ch4_in.fall(&yaw_down);
+    ch5_in.rise(&aux1_up);     ch5_in.fall(&aux1_down);
+    //ch6_in.rise(&aux2_up);     ch6_in.fall(&aux2_down);
+    
+    // pos初期値取得
+    // if pos 初期値(0,0) -> noVzかも
+    
+    // Get RC trim
+    for(int i=0;i<10;i++){
+        wait(.1);
+        PULTH_WIDTH_RC_TRIM_CH2 +=   g_pwm_pulse[0];
+        PULTH_WIDTH_RC_TRIM_CH3 +=   g_pwm_pulse[1];
+    }
+    PULTH_WIDTH_RC_TRIM_CH2 =   PULTH_WIDTH_RC_TRIM_CH2 / 10;
+    PULTH_WIDTH_RC_TRIM_CH3 =   PULTH_WIDTH_RC_TRIM_CH3 / 10;
+    
+    // Check RC trim
+    if(PULTH_WIDTH_RC_TRIM_CH2 < 450 || 550 < PULTH_WIDTH_RC_TRIM_CH2){
+        //pc.printf("%d\r\n", PULTH_WIDTH_RC_TRIM_CH2);
+        while(1){
+            led2 = 1;
+            led3 = 0;
+            wait(0.5);
+            led2 = 0;
+            led3 = 1;
+            wait(0.5);
+        }
+    }
+    
+    //////////////////// Stand by ////////////////////
+    
+    ch2_in.disable_irq();
+    ch3_in.disable_irq();
+    ch5_in.disable_irq();
+    
+    while(button < 0.5){
+        led1 = 1;
+        led4 = 1;
+        wait(0.5);
+        led1 = 0;
+        led4 = 0;
+        wait(0.5);
+    }
+    
+    wait(3);
+    
+    FILE *fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
+    
+    led1 = 1;
+    led4 = 1;
+    
+    while(button < 0.5){
+    //while(1){
+        
+        for(int i=0;i<5;i++){
+            
+            //////////////////// Get PWM ////////////////////
+            ch2_in.enable_irq();
+            ch3_in.enable_irq();
+            ch5_in.enable_irq();
+            
+            wait(.02);   // Interval to get pwm
+            
+            ch2_in.disable_irq();
+            ch3_in.disable_irq();
+            ch5_in.disable_irq();
+            
+            //////////////////// Get pos and vel ////////////////////
+            if(xbee.readable()){
+                // get vz data
+                int buf_char[SIZE_DATA] =   {0};
+                
+                // decode vz data
+                for(int j=0;j<SIZE_DATA;j++){   buf_char[j] =   (int)xbee.getc() - 48;  }
+                
+                //for(int i=0;i<SIZE_DATA;i++){  pc.printf("%5d ", buf_char[i]);  }
+                //pc.printf("\r\n");
+                
+                pos[1] = 1000*buf_char[0] + 100*buf_char[1] + 10*buf_char[2] + buf_char[3] - 5000;
+                pos[0] = 1000*buf_char[4] + 100*buf_char[5] + 10*buf_char[6] + buf_char[7] - 5000;
+                
+                vel[1] = 1000* buf_char[8] + 100* buf_char[9] + 10*buf_char[10] + buf_char[11] - 5000;
+                vel[0] = 1000*buf_char[12] + 100*buf_char[13] + 10*buf_char[14] + buf_char[15] - 5000;
+            }else{
+                
+                // しばらくこなかったらautoにならないフラグとか
+                
+            }
+            
+            //////////////////// Control law ////////////////////
+            if(500 < g_pwm_pulse[2]){
+                
+                // Constant velocity F/B
+                
+                //pulse_width_ch2_out =   0.001 + 0.000001 * g_pwm_pulse[0];
+                pulse_width_ch2_out =   0.001 + 0.000001 * PULTH_WIDTH_RC_TRIM_CH2 + (-1) * 0.000001 * GAIN_VEL_X * (VEL_X_TARGET - vel[0]);
+                
+                pulse_width_ch3_out =   0.001 + 0.000001 * g_pwm_pulse[1];
+                //pulse_width_ch3_out =   0.001 + PULTH_WIDTH_RC_TRIM_CH3 + GAIN_VEL_Z *  (-vel[1]);
+                
+            }else{
+                
+                // without RC-hack
+                pulse_width_ch2_out =   0.001 + 0.000001 * g_pwm_pulse[0];
+                pulse_width_ch3_out =   0.001 + 0.000001 * g_pwm_pulse[1];
+                
+            }
+            
+            // Limmiter for RC
+            if(pulse_width_ch2_out < LIMIT_PULTH_WIDTH_CH2_OUT_LOW){    pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_LOW;    }
+            if(LIMIT_PULTH_WIDTH_CH2_OUT_UP < pulse_width_ch2_out) {    pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_UP;     }
+            
+            ch2_out.pulsewidth(pulse_width_ch2_out);
+            ch3_out.pulsewidth(pulse_width_ch3_out);
+        
+        }
+        
+        
+        //for(int i=0;i<N_CHAN;i++){    pc.printf("%5d ", g_pwm_pulse[i]);  }
+        /*
+        pc.printf(" ");
+        for(int i=0;i<2;i++){   pc.printf("%5d ", pos[i]); }
+        for(int i=0;i<2;i++){   pc.printf("%5d ", vel[i]); }
+        pc.printf("\r\n");
+        */
+        fprintf(fp, "%f %f %5d  %5d %5d %5d %5d \r\n", pulse_width_ch2_out, pulse_width_ch3_out, g_pwm_pulse[2], pos[0], pos[1], vel[0], vel[1]);
+        
+    }
+    
+    ch2_out.pulsewidth(0.0015);
+    ch3_out.pulsewidth(0.0011);
+    
+    ch2_in.disable_irq();
+    ch3_in.disable_irq();
+    ch5_in.disable_irq();
+    
+    fclose(fp);
+    
+    led1 = 0;
+    led4 = 0;
+    led2 = 1;
+    led3 = 1;
+}
+
+
+//////////////////// RC interrupt (up) ////////////////////
+//void roll_up(){     t_roll.start();   }
+void pitch_up(){    t_pitch.start();  }
+void thr_up(){      t_thr.start();   }
+//void yaw_up(){      t_yaw.start();    }
+void aux1_up(){     t_aux1.start();   }
+//void aux2_up(){     t_aux2.start();   }
+
+
+//////////////////// RC interrupt (down) ////////////////////
+/*
+void roll_down(){
+    t_roll.stop();
+    g_pwm_pulse[]    =   (int)(t_roll.read()*1000000-1000);
+    t_roll.reset();
+}
+*/
+void pitch_down(){
+    t_pitch.stop();
+    g_pwm_pulse[0]    =   (int)(t_pitch.read()*1000000-1000);
+    t_pitch.reset();
+}
+
+void thr_down(){
+    t_thr.stop();
+    g_pwm_pulse[1]    =   (int)(t_thr.read()*1000000-1000);
+    t_thr.reset();
+}
+/*
+void yaw_down(){
+    t_yaw.stop();
+    g_pwm_pulse[]    =   (int)(t_yaw.read()*1000000-1000);
+    t_yaw.reset();
+}
+*/
+void aux1_down(){
+    t_aux1.stop();
+    g_pwm_pulse[2]    =   (int)(t_aux1.read()*1000000-1000);
+    t_aux1.reset();
+}
+/*
+void aux2_down(){
+    t_aux2.stop();
+    g_pwm_pulse[]    =   (int)(t_aux2.read()*1000000-1000);
+    t_aux2.reset();
+}
+*/

diff -r 000000000000 -r 72e0bf0dbe7c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 06 10:13:27 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file