part3.h

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "uLCD_4DGL.h"
00004 #include "PinDetect.h"
00005 #include <stdio.h>
00006 
00007 #define PI 3.14159
00008 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00009 
00010 class Line {
00011     public:
00012     int x;
00013     int y;
00014 };
00015 
00016 uLCD_4DGL uLCD(p28,p27,p30);
00017 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00018 Serial pc(USBTX, USBRX);
00019 
00020 PinDetect pb(p16, PullUp);
00021 
00022 int outer_radius = 15;
00023 int inner_radius = 10;
00024 
00025 int center = (int)floor(127.0 / 2.0);
00026 int max_diff = 66536;
00027 
00028 int radius = 10;
00029 int line_length = 20;
00030 
00031 float getAttitude(float ax, float ay, float az, float mx, float my, float mz)
00032 {
00033     float roll = atan2(ay, az);
00034     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00035 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00036     mx = -mx;
00037     float heading;
00038     if (my == 0.0)
00039         heading = (mx < 0.0) ? 180.0 : 0.0;
00040     else
00041         heading = atan2(mx, my)*360.0/(2.0*PI);
00042     //pc.printf("heading atan=%f \n\r",heading);
00043     heading -= DECLINATION; //correct for geo location
00044     if(heading>180.0) heading = heading - 360.0;
00045     else if(heading<-180.0) heading = 360.0 + heading;
00046     else if(heading<0.0) heading = 360.0  + heading;
00047 
00048 
00049     // Convert everything from radians to degrees:
00050     //heading *= 180.0 / PI;
00051     pitch *= 180.0 / PI;
00052     roll  *= 180.0 / PI;
00053 
00054 //    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00055     return heading;
00056 }
00057 
00058 float compensated_heading(float ax, float ay, float az, float mx, float my, float mz) {
00059     float p_ax, p_ay, p_az, p_mx, p_my, p_mz;
00060     p_ax = ax; p_ay = ay; p_az = az;
00061     p_mx = mx; p_my = my; p_mz = mz;
00062     
00063     float roll = atan2(p_ay, p_az);
00064     float iSin = sin(roll);
00065     float iCos = cos(roll);
00066     
00067     float f_my = (int)((p_my * iCos) - (p_mz * iSin)) >> 15;
00068     p_mz = (int)((p_my * iSin) + (p_mz * iCos)) >> 15;
00069     p_az = (int)((p_ay * iSin) + (p_az * iCos)) >> 15;
00070     
00071     float pitch = atan2(-p_ax, sqrt(p_ay * p_ay + p_az * p_az));
00072     iSin = sin(pitch);
00073     iCos = cos(pitch);
00074     if (iCos < 0) iCos = -iCos;
00075     
00076     float f_mx = (int)((p_mx * iCos) + (p_mz * iSin)) >> 15;
00077     
00078     float heading = atan2(-f_my, f_mx)*360 / (2*PI);
00079     heading -= DECLINATION; //correct for geo location
00080     if(heading>180.0) heading = heading - 360.0;
00081     else if(heading<-180.0) heading = 360.0 + heading;
00082     else if(heading<0.0) heading = 360.0  + heading;
00083     
00084     return heading;
00085 }
00086 
00087 int calc_x(float heading, int line_length) {
00088     float rad = (PI*heading/180);
00089     
00090     return (int)(line_length * cos(rad));
00091 }
00092 
00093 int calc_y(float heading, int line_length) {
00094     float rad = (PI*heading/180);
00095     
00096     return (int)(line_length * sin(rad));
00097 }
00098 
00099 int run_part3() {
00100     IMU.begin();
00101     if (!IMU.begin()) {
00102         pc.printf("Failed to communicate with LSM9DS1.\n");
00103     }
00104     IMU.calibrate(1);
00105     IMU.calibrateMag(0);
00106 
00107     pc.printf("finished imu begin");
00108     
00109     int center = (int)floor(127.0 / 2.0);
00110     int curr_x = center;
00111     int curr_y = center;
00112     
00113     while(1) {
00114         while (!IMU.accelAvailable());
00115         IMU.readAccel();
00116         
00117         uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
00118         uLCD.circle(center, center, outer_radius, WHITE);
00119         
00120         int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
00121         int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
00122         
00123         curr_x = center + y_diff;
00124         curr_y = center - x_diff;
00125         
00126         uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
00127         wait(0.01);
00128     }
00129 }
00130 
00131 int run_part3_EC() {
00132     IMU.begin();
00133     IMU.calibrate(1);
00134     IMU.calibrateMag(0);
00135     
00136     uLCD.background_color(BLACK);
00137     
00138     Line curr_line;
00139     curr_line.x = center;
00140     curr_line.y = center + line_length;
00141     
00142     while (1) {
00143         uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
00144         uLCD.filled_circle(center, center, radius, BLACK);
00145         uLCD.circle(center, center, radius, WHITE);
00146         
00147         IMU.readMag();
00148         IMU.readAccel();
00149         
00150         float heading = getAttitude(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
00151         
00152         curr_line.x = center + calc_x(heading, line_length);
00153         curr_line.y = center + calc_y(heading, line_length);
00154         
00155         uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
00156         
00157         char buffer[50];
00158         sprintf(buffer, "Heading: %f", heading);
00159         
00160         uLCD.text_string(buffer, 100, 10, 12, WHITE);
00161         
00162         wait(0.1);
00163     }
00164 }
00165 
00166 int run_pb_EC() {
00167     IMU.begin();
00168     IMU.calibrate(1);
00169     IMU.calibrateMag(0);
00170     
00171     int curr_x = center;
00172     int curr_y = center;
00173     
00174     uLCD.background_color(BLACK);
00175     
00176     Line curr_line;
00177     curr_line.x = center;
00178     curr_line.y = center + line_length;
00179     
00180     int prev_pb = pb;
00181     int mode = 0;
00182     
00183     while (1) {
00184         if (pb ^ prev_pb) {
00185             mode = !mode;
00186         }
00187         
00188         if (mode == 0) {
00189             uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
00190             uLCD.filled_circle(center, center, radius, BLACK);
00191             uLCD.circle(center, center, radius, WHITE);
00192             
00193             IMU.readMag();
00194             IMU.readAccel();
00195             
00196             float heading = compensated_heading(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
00197             
00198             curr_line.x = center + calc_x(heading, line_length);
00199             curr_line.y = center + calc_y(heading, line_length);
00200             
00201             uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
00202             
00203             char buffer[50];
00204             sprintf(buffer, "Heading: %f", heading);
00205             
00206             uLCD.text_string(buffer, 100, 10, 12, WHITE);
00207             
00208             wait(0.1);
00209         } else {
00210             while (!IMU.accelAvailable());
00211             IMU.readAccel();
00212             
00213             uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
00214             uLCD.circle(center, center, outer_radius, WHITE);
00215             
00216             int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
00217             int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
00218             
00219             curr_x = center + y_diff;
00220             curr_y = center - x_diff;
00221             
00222             uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
00223             wait(0.01);
00224         }
00225     }
00226 }
00227 
00228 int run_time_EC() {
00229     set_time(1580736096);
00230     
00231     int center = (int)floor(127.0 / 2.0);
00232     
00233     while (1) {
00234         time_t seconds = time(NULL);
00235         
00236         char buffer[50];
00237         sprintf(buffer, "Time: %s", ctime(&seconds));
00238         
00239         uLCD.text_string(buffer, center, center, 10, RED);
00240         
00241         wait(0.2);
00242         uLCD.rectangle(0, 0, 127, 127, BLACK);
00243     }
00244 }