4180 Lab 2
Dependencies: mbed wave_player Servo 4DGL-uLCD-SE Motor SDFileSystem LSM9DS1_Library_cal PinDetect X_NUCLEO_53L0A1
part3.h
- Committer:
- emilywilson
- Date:
- 2020-02-04
- Revision:
- 2:de355b6fbd87
- Parent:
- 1:6d8f645530b8
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); } }