main.cpp

00001  /* Copyright (c) 2012 - 2013 Gaëtan PLEYBER
00002  *
00003  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED 
00004  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
00005  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
00006  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00007  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
00008  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
00009  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 
00010  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 
00011  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00012  */
00013  
00014  /*
00015  * Description
00016  * Input
00017  * Output
00018  */
00019 
00020 #include "mbed.h"                    
00021 #include "mbos.h"
00022 #include "Module_Communication.h"
00023 #include "Module_Mouvement.h"
00024 #include "Acc_Giro.h"
00025  
00026 #define TASK1_ID                1       // Id for task 1 (idle task is 0)
00027 #define TASK1_PRIO              50      // priority for task 1
00028 #define TASK1_STACK_SZ          32      // stack size for task 1 in words 
00029 #define TASK2_ID                2       // Id for task 2 
00030 #define TASK2_PRIO              60      // priority for task 2
00031 #define TASK2_STACK_SZ          32      // stack size for task 2 in words 
00032 #define TIMER0_ID               0       // Id for timer 0
00033 #define TIMER0_PERIOD           1000    // Time period in milliseconds
00034 #define TIMER0_EVENT            1       // Event flag (1 << 0)
00035 #define T1_TO_T2_EVENT          2       // Event flag (1 << 1)
00036 
00037 void task1(void);                       // task function prototypes
00038 void task2(void);
00039 
00040 DigitalOut led1(LED1);
00041 DigitalOut led2(LED2);
00042 mbos os(2, 1);                          // Instantiate mbos with 2 tasks & 1 timer 
00043 Acc_Giro Acc_Giro_test; 
00044 Serial pc(USBTX, USBRX);  
00045 
00046 int main(void)
00047 {
00048     //Initialize inertial sensors.
00049     Acc_Giro_test.initializeAccelerometer();
00050     Acc_Giro_test.initializeGyroscope();
00051           
00052     Timer tmr;
00053     tmr.start();
00054     
00055     pc.printf("test started");
00056     Acc_Giro_test.calibrate=1;
00057     
00058     while(true)
00059     {
00060         //Net::poll();
00061         if(tmr.read() > 0.2){
00062            // led4=!led4;
00063             tmr.reset();
00064             wait(0.5);
00065         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()));
00066            
00067         }
00068         
00069         if(Acc_Giro_test.calibrate && !Acc_Giro_test.calibrated){
00070             Acc_Giro_test.calibrateAccelerometer();
00071             Acc_Giro_test.calibrateGyroscope();
00072             led2 = 1;
00073             Acc_Giro_test.calibrated = 1;
00074             Acc_Giro_test.start = 1;
00075           pc.printf("Calibrated\n");
00076         }
00077         
00078         
00079         if(Acc_Giro_test.calibrated && Acc_Giro_test.start && !Acc_Giro_test.started){
00080          
00081             //Accelerometer data rate is 500Hz, so we'll sample at this speed.
00082             Acc_Giro_test.accelerometerTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleAccelerometer, 0.002);
00083             //Gyroscope data rate is 500Hz, so we'll sample at this speed.
00084             Acc_Giro_test.gyroscopeTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleGyroscope,  0.002);
00085             //Update the filter variables at the correct rate.
00086             Acc_Giro_test.filterTicker.attach(&Acc_Giro_test, &Acc_Giro::filter, FILTER_RATE);
00087             //Acc_Giro_test.dataTicker.attach(&Acc_Giro_test, &Acc_Giro::dataSender, 0.2);
00088            // Acc_Giro_test.algorithmTicker.attach(&algorithm, 0.001);
00089             Acc_Giro_test.started = 1;
00090         }  
00091         
00092         
00093     }  
00094 }
00095 
00096 void task1(void)
00097 {
00098     os.SetTimer(TIMER0_ID, TIMER0_PERIOD, TIMER0_PERIOD);
00099     while(1){
00100         os.WaitEvent(TIMER0_EVENT);
00101         led1 = !led1;
00102         os.SetEvent(T1_TO_T2_EVENT, TASK2_ID);
00103     }
00104 }
00105 
00106 void task2(void)
00107 {
00108     while(1){
00109         os.WaitEvent(T1_TO_T2_EVENT);
00110         led2 = 1;
00111         wait_ms(100);
00112         led2 = 0;
00113    }
00114 }
00115 /*@endcode
00116  */