main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "Servo.h"
00004 #include "FXOS8700Q.h"
00005 #include "math.h"
00006 #define M_PI 3.1415926535
00007 
00008 Serial pc(USBTX, USBRX);
00009 Serial device (PTC15,PTC14);
00010 HCSR04  usensor(A4,A5);
00011 I2C i2c(PTE25, PTE24);
00012 FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1);
00013 DigitalOut RED (LED1);
00014 DigitalOut GREEN (LED2);
00015 DigitalOut BLUE (LED3);
00016 int distance;              //Variable to store distance from an object
00017 int state = 0;
00018 double pitch, roll;
00019 const float alpha = 0.5;
00020 double fxg = 0;
00021 double fyg = 0;
00022 double fzg = 0;
00023 motion_data_counts_t acc_raw;
00024     
00025 void filter_accelerometer()
00026 {
00027     double xg, yg, zg;
00028     
00029     acc.getAxis(acc_raw);
00030     
00031     xg = acc_raw.x;
00032     yg = acc_raw.y;
00033     zg = acc_raw.z;
00034     
00035         // low pass filter
00036     fxg = xg * alpha + (fxg * (1.0 - alpha));
00037     fyg = yg * alpha + (fyg * (1.0 - alpha));
00038     fzg = zg * alpha + (fzg * (1.0 - alpha));
00039 
00040         // pitch and roll calculations
00041     roll  = (atan2(-fyg, fzg)*180.0)/M_PI;
00042     pitch = (atan2(fxg, sqrt(fyg*fyg + fzg*fzg))*180.0)/M_PI;
00043     printf(" %0.2f %0.2f \r\n", roll, pitch);
00044     }
00045 
00046 int main()
00047 {  
00048 acc.enable();
00049     GREEN = 1;
00050     BLUE = 1;
00051     RED = 1;
00052     while(1) 
00053     {
00054         usensor.start();
00055         wait_ms(0.0025); 
00056         distance = usensor.get_dist_cm();
00057         wait(0.5);
00058         pc.printf(" distance = %d\r\n", distance);
00059         if(distance <= 9 )
00060         {
00061             device.printf("ALERT VEHICLE");  
00062         }
00063        filter_accelerometer();
00064        if (roll > 22.00 && pitch > 3.00 )
00065        {
00066          GREEN = BLUE = 1;
00067          RED = 0;
00068          
00069          wait(0.5);
00070          RED = 1;
00071        }
00072         if(device.readable() > 0) 
00073         {
00074             state = device.getc();
00075             pc.printf("Instruction: %i\n\r", state);    
00076         } 
00077         if (state == '1')
00078         {
00079             RED = BLUE = 1;
00080             GREEN = 0;
00081             wait(0.3);
00082             GREEN = 1;
00083             wait(0.3);
00084         }
00085         else if (state == '2') 
00086         {
00087             RED = GREEN = 1;
00088             BLUE = 0;
00089             wait(0.3);
00090             BLUE = 1;
00091             wait (0.3);
00092         }
00093         else if (state == '3') {
00094             RED = GREEN = 1;
00095             BLUE = 1;
00096             
00097         }
00098         
00099     }
00100 }