Diff: main.cpp

Revision:

6:07f4aaae5339

Parent:

5:12e37af16f2e

Child:

7:ec00db826804

--- a/main.cpp	Mon Sep 03 03:44:13 2012 +0000
+++ b/main.cpp	Thu Sep 20 13:00:24 2012 +0000
@@ -1,83 +1,73 @@
 #include "mbed.h"
-#include "ADXL345_I2C.h"
 #include "ITG3200.h"
 #include "HMC5883L.h"
-#include "SDFileSystem.h"
 
-ADXL345_I2C accelerometer(p9, p10);
+#define N    3
+#define chan 6
+
 ITG3200 gyro(p9, p10);
 HMC5883L compass(p9, p10);
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
-DigitalIn stop(p20);
+InterruptIn ch1(p13);       // aileron
+InterruptIn ch2(p14);       // elevator
+InterruptIn ch3(p15);       // throttle
+InterruptIn ch4(p16);       // rudder
+InterruptIn ch5(p17);       // switch1
+InterruptIn ch6(p18);       // switch2
+AnalogIn sonar(p19);        // sonar
+PwmOut ch1_out(p21);
+PwmOut ch2_out(p22);
+PwmOut ch3_out(p23);
+PwmOut ch4_out(p24);
+PwmOut ch5_out(p25);
+PwmOut ch6_out(p26);
 Serial pc(USBTX, USBRX);
 
-#define N 3
+Timer t_aile;       Timer t_elev;       Timer t_thro;       Timer t_rudd;       Timer t_switch1;        Timer t_switch2;
 
-int preparing_acc();
+void aile_up();     void elev_up();     void thro_up();     void rudd_up();     void switch1_up();      void switch2_up();
+void aile_down();   void elev_down();   void thro_down();   void rudd_down();   void switch1_down();    void switch2_down();
+
+double pwm_pulse[chan]      =   {0.001,0.001,0.001,0.001,0.001,0.001};        // aile, elev, thro, rudd, switch1, switch2
 
 int main(){ 
     
-    float dt = 0.01;
+    float dt = 0.1;
     float t  = 0;    
     pc.baud(921600);
-    int    bitAcc[N]    =   {0};    // Buffer of the accelerometer
+    
+    double range        =   0; 
     int    getMag[N]    =   {0};    // Buffer of the compass
-    double Acc  [N]    =   {0};
     double Gyro [N]    =   {0};
     int    Mag  [N]    =   {0};
     
-    // SD file system
-    mkdir("/sd/mydir", 0777);
-     
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if( fp==NULL ){     error("Could not open file for write\n");   }
+    double servo[6]    =   {0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
+    
+    ch1_out.period(0.02);    ch2_out.period(0.02);    ch3_out.period(0.02);    ch4_out.period(0.02);    ch5_out.period(0.02);    ch6_out.period(0.02);
+    // pulsewidth 0.001~0.002
+    ch1_out.pulsewidth(0.001);   ch2_out.pulsewidth(0.001);   ch3_out.pulsewidth(0.001);   ch4_out.pulsewidth(0.001);   ch5_out.pulsewidth(0.001);   ch6_out.pulsewidth(0.001);
     
     // *** start up ***
-    // These are here to test whether any of the initialization fails. It will print the failure.
-    if (accelerometer.setPowerControl(0x00)){
-        pc.printf("didn't intitialize power control\n\r"); 
-        return 0;
-    }
-    // Full resolution, +/-16g, 4mg/LSB.
-    wait(.001);
-     
-    if(accelerometer.setDataFormatControl(0x0B)){
-        pc.printf("didn't set data format\n\r");
-        return 0;
-    }
-    wait(.001);
-     
-    // 3.2kHz data rate.
-    if(accelerometer.setDataRate(ADXL345_3200HZ)){
-        pc.printf("didn't set data rate\n\r");
-        return 0;
-    }
-    wait(.001);
-      
-    if(accelerometer.setPowerControl(MeasurementMode)) {
-        pc.printf("didn't set the power control to measurement\n\r"); 
-        return 0;
-    }
-
     gyro.setLpBandwidth(LPFBW_42HZ);
     compass.setDefault();
     wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
     // *** start up ***
+    
+    // Reciever function
+    ch1.rise(&aile_up);     ch1.fall(&aile_down);
+    ch2.rise(&elev_up);     ch2.fall(&elev_down);
+    ch3.rise(&thro_up);     ch3.fall(&thro_down);
+    ch4.rise(&rudd_up);     ch4.fall(&rudd_down);
+    ch5.rise(&switch1_up);  ch5.fall(&switch1_down);
+    ch6.rise(&switch2_up);  ch6.fall(&switch2_down);
+    
+    pc.printf("\n\rHiko-robo!!\n\r");
+    
+    while(1){
         
-    pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
-    pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
-
-    while( stop==0 ){
-    //for( int i=0;i<10000;i++ ){
-    
+        range   =   sonar;
+        
         // Updating accelerometer and compass
-        accelerometer.getOutput(bitAcc);
         compass.readData(getMag);
-        
-        // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
-        Acc[0]  =  ((int16_t)bitAcc[0])*0.004;
-        Acc[1]  =  ((int16_t)bitAcc[1])*0.004;
-        Acc[2]  =  ((int16_t)bitAcc[2])*0.004;
 
         Gyro[0] =  (gyro.getGyroX())/14.375;
         Gyro[1] =  (gyro.getGyroY())/14.375;
@@ -87,27 +77,58 @@
         Mag[1]  =  (int16_t)getMag[1];
         Mag[2]  =  (int16_t)getMag[2];
         
-        // Low pass filter for acc
-        //for ( int i=0;i<N;i++ ){
-        //    if( -0.05<Acc[i] && Acc[i]<0.05 ){    Acc[i]=0;  }
-        //}
+        for( int i=0;i<6;i++ ){     servo[i] =   pwm_pulse[i];  }
         
-        // Low pass filter for gyro
-        //for ( int i=0;i<N;i++ ){
-        //    if( -1.0<Gyro[i] && Gyro[i]<1.0 ){    Gyro[i]=0;  }
-        //}
-
-        //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]);
-        pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
-        //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]);
-        fprintf(fp, "%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
+        ch1_out.pulsewidth(servo[0]);  ch2_out.pulsewidth(servo[1]);  ch3_out.pulsewidth(servo[2]);  ch4_out.pulsewidth(servo[3]);  ch5_out.pulsewidth(servo[4]);  ch6_out.pulsewidth(servo[5]);
+        
+        pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, Gyro[0], Gyro[1], Gyro[2], range, pwm_pulse[0]*1000, pwm_pulse[1]*1000, pwm_pulse[2]*1000, pwm_pulse[3]*1000, pwm_pulse[4]*1000, pwm_pulse[5]*1000);
         
         t += dt;
         wait(dt);
         
     }
     
-    fclose(fp);
-    
  }
- 
\ No newline at end of file
+
+void aile_up(){     t_aile.start();    }
+void elev_up(){     t_elev.start();    }
+void thro_up(){     t_thro.start();    }
+void rudd_up(){     t_rudd.start();    }
+void switch1_up(){  t_switch1.start(); }
+void switch2_up(){  t_switch2.start(); }
+
+void aile_down(){
+    t_aile.stop();
+    pwm_pulse[0]        =   t_aile.read();
+    t_aile.reset();
+}
+
+void elev_down(){
+    t_elev.stop();
+    pwm_pulse[1]        =   t_elev.read();
+    t_elev.reset();
+}
+
+void thro_down(){
+    t_thro.stop();
+    pwm_pulse[2]        =   t_thro.read();
+    t_thro.reset();
+}
+
+void rudd_down(){
+    t_rudd.stop();
+    pwm_pulse[3]        =   t_rudd.read();
+    t_rudd.reset();
+}
+
+void switch1_down(){
+    t_switch1.stop();
+    pwm_pulse[4]   =   t_switch1.read();
+    t_switch1.reset();
+}
+
+void switch2_down(){
+    t_switch2.stop();
+    pwm_pulse[5]   =   t_switch2.read();
+    t_switch2.reset();
+}
\ No newline at end of file