Arnaud Suire / Labo_TRSE_Drone

ajout module_mouvement

Dependencies:   mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID

Fork of Labo_TRSE_Drone by HERBERT Nicolas

main.cpp@38:7ab036d94678, 2014-02-26 (annotated)

Committer:
arnaudsuire
Date:
Wed Feb 26 08:47:14 2014 +0000
Revision:
38:7ab036d94678
Parent:
37:e1ad11fe3fe4 
arnaud

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaetan 10:c8d73680b9fd 1 /* Copyright (c) 2012 - 2013 Gaëtan PLEYBER
Gaetan 10:c8d73680b9fd 2 *
Gaetan 10:c8d73680b9fd 3 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED
Gaetan 10:c8d73680b9fd 4 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
Gaetan 10:c8d73680b9fd 5 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
Gaetan 10:c8d73680b9fd 6 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
Gaetan 10:c8d73680b9fd 7 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
Gaetan 10:c8d73680b9fd 8 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
Gaetan 10:c8d73680b9fd 9 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
Gaetan 10:c8d73680b9fd 10 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
Gaetan 10:c8d73680b9fd 11 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Gaetan 10:c8d73680b9fd 12 */
Gaetan 10:c8d73680b9fd 13
Gaetan 10:c8d73680b9fd 14 /*
Gaetan 10:c8d73680b9fd 15 * Description
Gaetan 10:c8d73680b9fd 16 * Input
Gaetan 10:c8d73680b9fd 17 * Output
Gaetan 10:c8d73680b9fd 18 */
Gaetan 10:c8d73680b9fd 19
Gaetan 6:0e3d95e9885e 20 #include "mbed.h"
Gaetan 6:0e3d95e9885e 21 #include "mbos.h"
IngesupMbed01 31:a0800d3da787 22 #include "Module_Communication.h"
arnaudsuire 36:660be809a49d 23 #include "Module_Mouvement.h"
arnaudsuire 37:e1ad11fe3fe4 24 #include "Acc_Giro.h"
Gaetan 6:0e3d95e9885e 25
Gaetan 6:0e3d95e9885e 26 #define TASK1_ID 1 // Id for task 1 (idle task is 0)
Gaetan 6:0e3d95e9885e 27 #define TASK1_PRIO 50 // priority for task 1
Gaetan 6:0e3d95e9885e 28 #define TASK1_STACK_SZ 32 // stack size for task 1 in words
Gaetan 6:0e3d95e9885e 29 #define TASK2_ID 2 // Id for task 2
Gaetan 6:0e3d95e9885e 30 #define TASK2_PRIO 60 // priority for task 2
Gaetan 6:0e3d95e9885e 31 #define TASK2_STACK_SZ 32 // stack size for task 2 in words
Gaetan 6:0e3d95e9885e 32 #define TIMER0_ID 0 // Id for timer 0
Gaetan 6:0e3d95e9885e 33 #define TIMER0_PERIOD 1000 // Time period in milliseconds
Gaetan 6:0e3d95e9885e 34 #define TIMER0_EVENT 1 // Event flag (1 << 0)
Gaetan 6:0e3d95e9885e 35 #define T1_TO_T2_EVENT 2 // Event flag (1 << 1)
Gaetan 6:0e3d95e9885e 36
Gaetan 6:0e3d95e9885e 37 void task1(void); // task function prototypes
Gaetan 6:0e3d95e9885e 38 void task2(void);
Gaetan 6:0e3d95e9885e 39
Gaetan 6:0e3d95e9885e 40 DigitalOut led1(LED1);
Gaetan 6:0e3d95e9885e 41 DigitalOut led2(LED2);
arnaudsuire 36:660be809a49d 42 mbos os(2, 1); // Instantiate mbos with 2 tasks & 1 timer
arnaudsuire 37:e1ad11fe3fe4 43 Acc_Giro Acc_Giro_test;
arnaudsuire 37:e1ad11fe3fe4 44 Serial pc(USBTX, USBRX);
Gaetan 6:0e3d95e9885e 45
Gaetan 6:0e3d95e9885e 46 int main(void)
Gaetan 6:0e3d95e9885e 47 {
arnaudsuire 37:e1ad11fe3fe4 48 //Initialize inertial sensors.
arnaudsuire 37:e1ad11fe3fe4 49 Acc_Giro_test.initializeAccelerometer();
arnaudsuire 37:e1ad11fe3fe4 50 Acc_Giro_test.initializeGyroscope();
arnaudsuire 37:e1ad11fe3fe4 51
arnaudsuire 37:e1ad11fe3fe4 52 Timer tmr;
arnaudsuire 37:e1ad11fe3fe4 53 tmr.start();
arnaudsuire 37:e1ad11fe3fe4 54
arnaudsuire 37:e1ad11fe3fe4 55 pc.printf("test started");
arnaudsuire 37:e1ad11fe3fe4 56 Acc_Giro_test.calibrate=1;
arnaudsuire 36:660be809a49d 57
arnaudsuire 37:e1ad11fe3fe4 58 while(true)
arnaudsuire 37:e1ad11fe3fe4 59 {
arnaudsuire 37:e1ad11fe3fe4 60 //Net::poll();
arnaudsuire 37:e1ad11fe3fe4 61 if(tmr.read() > 0.2){
arnaudsuire 37:e1ad11fe3fe4 62 // led4=!led4;
arnaudsuire 37:e1ad11fe3fe4 63 tmr.reset();
arnaudsuire 37:e1ad11fe3fe4 64 wait(0.5);
arnaudsuire 37:e1ad11fe3fe4 65 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()));
arnaudsuire 37:e1ad11fe3fe4 66
arnaudsuire 37:e1ad11fe3fe4 67 }
arnaudsuire 37:e1ad11fe3fe4 68
arnaudsuire 37:e1ad11fe3fe4 69 if(Acc_Giro_test.calibrate && !Acc_Giro_test.calibrated){
arnaudsuire 37:e1ad11fe3fe4 70 Acc_Giro_test.calibrateAccelerometer();
arnaudsuire 37:e1ad11fe3fe4 71 Acc_Giro_test.calibrateGyroscope();
arnaudsuire 37:e1ad11fe3fe4 72 led2 = 1;
arnaudsuire 37:e1ad11fe3fe4 73 Acc_Giro_test.calibrated = 1;
arnaudsuire 37:e1ad11fe3fe4 74 Acc_Giro_test.start = 1;
arnaudsuire 37:e1ad11fe3fe4 75 pc.printf("Calibrated\n");
arnaudsuire 37:e1ad11fe3fe4 76 }
arnaudsuire 37:e1ad11fe3fe4 77
arnaudsuire 37:e1ad11fe3fe4 78
arnaudsuire 37:e1ad11fe3fe4 79 if(Acc_Giro_test.calibrated && Acc_Giro_test.start && !Acc_Giro_test.started){
arnaudsuire 37:e1ad11fe3fe4 80
arnaudsuire 37:e1ad11fe3fe4 81 //Accelerometer data rate is 500Hz, so we'll sample at this speed.
arnaudsuire 37:e1ad11fe3fe4 82 Acc_Giro_test.accelerometerTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleAccelerometer, 0.002);
arnaudsuire 37:e1ad11fe3fe4 83 //Gyroscope data rate is 500Hz, so we'll sample at this speed.
arnaudsuire 37:e1ad11fe3fe4 84 Acc_Giro_test.gyroscopeTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleGyroscope, 0.002);
arnaudsuire 37:e1ad11fe3fe4 85 //Update the filter variables at the correct rate.
arnaudsuire 37:e1ad11fe3fe4 86 Acc_Giro_test.filterTicker.attach(&Acc_Giro_test, &Acc_Giro::filter, FILTER_RATE);
arnaudsuire 37:e1ad11fe3fe4 87 //Acc_Giro_test.dataTicker.attach(&Acc_Giro_test, &Acc_Giro::dataSender, 0.2);
arnaudsuire 37:e1ad11fe3fe4 88 // Acc_Giro_test.algorithmTicker.attach(&algorithm, 0.001);
arnaudsuire 37:e1ad11fe3fe4 89 Acc_Giro_test.started = 1;
arnaudsuire 37:e1ad11fe3fe4 90 }
arnaudsuire 37:e1ad11fe3fe4 91
arnaudsuire 37:e1ad11fe3fe4 92
arnaudsuire 37:e1ad11fe3fe4 93 }
NicolasH 0:8d93a5e4e4ab 94 }
Gaetan 6:0e3d95e9885e 95
Gaetan 6:0e3d95e9885e 96 void task1(void)
Gaetan 6:0e3d95e9885e 97 {
Gaetan 6:0e3d95e9885e 98 os.SetTimer(TIMER0_ID, TIMER0_PERIOD, TIMER0_PERIOD);
Gaetan 6:0e3d95e9885e 99 while(1){
Gaetan 6:0e3d95e9885e 100 os.WaitEvent(TIMER0_EVENT);
Gaetan 6:0e3d95e9885e 101 led1 = !led1;
Gaetan 6:0e3d95e9885e 102 os.SetEvent(T1_TO_T2_EVENT, TASK2_ID);
Gaetan 6:0e3d95e9885e 103 }
Gaetan 6:0e3d95e9885e 104 }
Gaetan 6:0e3d95e9885e 105
Gaetan 6:0e3d95e9885e 106 void task2(void)
Gaetan 6:0e3d95e9885e 107 {
Gaetan 6:0e3d95e9885e 108 while(1){
Gaetan 6:0e3d95e9885e 109 os.WaitEvent(T1_TO_T2_EVENT);
Gaetan 6:0e3d95e9885e 110 led2 = 1;
Gaetan 6:0e3d95e9885e 111 wait_ms(100);
Gaetan 6:0e3d95e9885e 112 led2 = 0;
Gaetan 6:0e3d95e9885e 113 }
Gaetan 6:0e3d95e9885e 114 }
Gaetan 6:0e3d95e9885e 115 /*@endcode
Gaetan 6:0e3d95e9885e 116 */