File content as of revision 2:de355b6fbd87:

#include "mbed.h"
#include "LSM9DS1.h"
#include "uLCD_4DGL.h"
#include "PinDetect.h"
#include <stdio.h>

#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.

class Line {
    public:
    int x;
    int y;
};

uLCD_4DGL uLCD(p28,p27,p30);
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
Serial pc(USBTX, USBRX);

PinDetect pb(p16, PullUp);

int outer_radius = 15;
int inner_radius = 10;

int center = (int)floor(127.0 / 2.0);
int max_diff = 66536;

int radius = 10;
int line_length = 20;

float getAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
    float roll = atan2(ay, az);
    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
    mx = -mx;
    float heading;
    if (my == 0.0)
        heading = (mx < 0.0) ? 180.0 : 0.0;
    else
        heading = atan2(mx, my)*360.0/(2.0*PI);
    //pc.printf("heading atan=%f \n\r",heading);
    heading -= DECLINATION; //correct for geo location
    if(heading>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;


    // Convert everything from radians to degrees:
    //heading *= 180.0 / PI;
    pitch *= 180.0 / PI;
    roll  *= 180.0 / PI;

//    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
    return heading;
}

float compensated_heading(float ax, float ay, float az, float mx, float my, float mz) {
    float p_ax, p_ay, p_az, p_mx, p_my, p_mz;
    p_ax = ax; p_ay = ay; p_az = az;
    p_mx = mx; p_my = my; p_mz = mz;
    
    float roll = atan2(p_ay, p_az);
    float iSin = sin(roll);
    float iCos = cos(roll);
    
    float f_my = (int)((p_my * iCos) - (p_mz * iSin)) >> 15;
    p_mz = (int)((p_my * iSin) + (p_mz * iCos)) >> 15;
    p_az = (int)((p_ay * iSin) + (p_az * iCos)) >> 15;
    
    float pitch = atan2(-p_ax, sqrt(p_ay * p_ay + p_az * p_az));
    iSin = sin(pitch);
    iCos = cos(pitch);
    if (iCos < 0) iCos = -iCos;
    
    float f_mx = (int)((p_mx * iCos) + (p_mz * iSin)) >> 15;
    
    float heading = atan2(-f_my, f_mx)*360 / (2*PI);
    heading -= DECLINATION; //correct for geo location
    if(heading>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;
    
    return heading;
}

int calc_x(float heading, int line_length) {
    float rad = (PI*heading/180);
    
    return (int)(line_length * cos(rad));
}

int calc_y(float heading, int line_length) {
    float rad = (PI*heading/180);
    
    return (int)(line_length * sin(rad));
}

int run_part3() {
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0);

    pc.printf("finished imu begin");
    
    int center = (int)floor(127.0 / 2.0);
    int curr_x = center;
    int curr_y = center;
    
    while(1) {
        while (!IMU.accelAvailable());
        IMU.readAccel();
        
        uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
        uLCD.circle(center, center, outer_radius, WHITE);
        
        int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
        int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
        
        curr_x = center + y_diff;
        curr_y = center - x_diff;
        
        uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
        wait(0.01);
    }
}

int run_part3_EC() {
    IMU.begin();
    IMU.calibrate(1);
    IMU.calibrateMag(0);
    
    uLCD.background_color(BLACK);
    
    Line curr_line;
    curr_line.x = center;
    curr_line.y = center + line_length;
    
    while (1) {
        uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
        uLCD.filled_circle(center, center, radius, BLACK);
        uLCD.circle(center, center, radius, WHITE);
        
        IMU.readMag();
        IMU.readAccel();
        
        float heading = getAttitude(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
        
        curr_line.x = center + calc_x(heading, line_length);
        curr_line.y = center + calc_y(heading, line_length);
        
        uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
        
        char buffer[50];
        sprintf(buffer, "Heading: %f", heading);
        
        uLCD.text_string(buffer, 100, 10, 12, WHITE);
        
        wait(0.1);
    }
}

int run_pb_EC() {
    IMU.begin();
    IMU.calibrate(1);
    IMU.calibrateMag(0);
    
    int curr_x = center;
    int curr_y = center;
    
    uLCD.background_color(BLACK);
    
    Line curr_line;
    curr_line.x = center;
    curr_line.y = center + line_length;
    
    int prev_pb = pb;
    int mode = 0;
    
    while (1) {
        if (pb ^ prev_pb) {
            mode = !mode;
        }
        
        if (mode == 0) {
            uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
            uLCD.filled_circle(center, center, radius, BLACK);
            uLCD.circle(center, center, radius, WHITE);
            
            IMU.readMag();
            IMU.readAccel();
            
            float heading = compensated_heading(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
            
            curr_line.x = center + calc_x(heading, line_length);
            curr_line.y = center + calc_y(heading, line_length);
            
            uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
            
            char buffer[50];
            sprintf(buffer, "Heading: %f", heading);
            
            uLCD.text_string(buffer, 100, 10, 12, WHITE);
            
            wait(0.1);
        } else {
            while (!IMU.accelAvailable());
            IMU.readAccel();
            
            uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
            uLCD.circle(center, center, outer_radius, WHITE);
            
            int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
            int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
            
            curr_x = center + y_diff;
            curr_y = center - x_diff;
            
            uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
            wait(0.01);
        }
    }
}

int run_time_EC() {
    set_time(1580736096);
    
    int center = (int)floor(127.0 / 2.0);
    
    while (1) {
        time_t seconds = time(NULL);
        
        char buffer[50];
        sprintf(buffer, "Time: %s", ctime(&seconds));
        
        uLCD.text_string(buffer, center, center, 10, RED);
        
        wait(0.2);
        uLCD.rectangle(0, 0, 127, 127, BLACK);
    }
}