ajout module_mouvement
Dependencies: mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID
Fork of Labo_TRSE_Drone by
Diff: main.cpp
- Revision:
- 37:e1ad11fe3fe4
- Parent:
- 36:660be809a49d
--- a/main.cpp Wed Nov 20 10:47:38 2013 +0000 +++ b/main.cpp Wed Feb 26 08:46:22 2014 +0000 @@ -17,25 +17,11 @@ * Output */ -/*#include "mbed.h" -#include "mbos.h" - -DigitalOut myled(LED1); - -int main() { - while(1) { - myled = 1; - wait(0.2); - myled = 0; - wait(0.2); - - //modification - } -}*/ #include "mbed.h" #include "mbos.h" #include "Module_Communication.h" #include "Module_Mouvement.h" +#include "Acc_Giro.h" #define TASK1_ID 1 // Id for task 1 (idle task is 0) #define TASK1_PRIO 50 // priority for task 1 @@ -54,23 +40,57 @@ DigitalOut led1(LED1); DigitalOut led2(LED2); mbos os(2, 1); // Instantiate mbos with 2 tasks & 1 timer -ModuleMouvement test; +Acc_Giro Acc_Giro_test; +Serial pc(USBTX, USBRX); int main(void) { -/* - // Configure tasks and timers - os.CreateTask(TASK1_ID, TASK1_PRIO, TASK1_STACK_SZ, task1); - os.CreateTask(TASK2_ID, TASK2_PRIO, TASK2_STACK_SZ, task2); - os.CreateTimer(TIMER0_ID, TIMER0_EVENT, TASK1_ID); - // Start mbos - os.Start(); - // never return! - */ - test.TestMotor(); - C_ModuleCommunication com = C_ModuleCommunication(); - com.receptionDeTrame(); + //Initialize inertial sensors. + Acc_Giro_test.initializeAccelerometer(); + Acc_Giro_test.initializeGyroscope(); + + Timer tmr; + tmr.start(); + + pc.printf("test started"); + Acc_Giro_test.calibrate=1; + while(true) + { + //Net::poll(); + if(tmr.read() > 0.2){ + // led4=!led4; + tmr.reset(); + wait(0.5); + pc.printf("Ax:%f Ay:%f Az:%f || Gx:%f Gy:%f Gz:%f\n", Acc_Giro_test.a_x, Acc_Giro_test.a_y, Acc_Giro_test.a_z, toDegrees(Acc_Giro_test.imuFilter->getRoll()), toDegrees(Acc_Giro_test.imuFilter->getPitch()), toDegrees(Acc_Giro_test.imuFilter->getYaw())); + + } + + if(Acc_Giro_test.calibrate && !Acc_Giro_test.calibrated){ + Acc_Giro_test.calibrateAccelerometer(); + Acc_Giro_test.calibrateGyroscope(); + led2 = 1; + Acc_Giro_test.calibrated = 1; + Acc_Giro_test.start = 1; + pc.printf("Calibrated\n"); + } + + + if(Acc_Giro_test.calibrated && Acc_Giro_test.start && !Acc_Giro_test.started){ + + //Accelerometer data rate is 500Hz, so we'll sample at this speed. + Acc_Giro_test.accelerometerTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleAccelerometer, 0.002); + //Gyroscope data rate is 500Hz, so we'll sample at this speed. + Acc_Giro_test.gyroscopeTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleGyroscope, 0.002); + //Update the filter variables at the correct rate. + Acc_Giro_test.filterTicker.attach(&Acc_Giro_test, &Acc_Giro::filter, FILTER_RATE); + //Acc_Giro_test.dataTicker.attach(&Acc_Giro_test, &Acc_Giro::dataSender, 0.2); + // Acc_Giro_test.algorithmTicker.attach(&algorithm, 0.001); + Acc_Giro_test.started = 1; + } + + + } } void task1(void)