Emily Wilson / Mbed 2 deprecated ECE4180Lab2

4180 Lab 2

Dependencies:   mbed wave_player Servo 4DGL-uLCD-SE Motor SDFileSystem LSM9DS1_Library_cal PinDetect X_NUCLEO_53L0A1

part3.h@2:de355b6fbd87, 2020-02-04 (annotated)

Committer:
emilywilson
Date:
Tue Feb 04 20:00:52 2020 +0000
Revision:
2:de355b6fbd87
Parent:
1:6d8f645530b8 
parts 10-14 and extra credit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilywilson 0:90df82d4732d 1 #include "mbed.h"
emilywilson 1:6d8f645530b8 2 #include "LSM9DS1.h"
emilywilson 0:90df82d4732d 3 #include "uLCD_4DGL.h"
emilywilson 2:de355b6fbd87 4 #include "PinDetect.h"
emilywilson 1:6d8f645530b8 5 #include <stdio.h>
emilywilson 0:90df82d4732d 6
emilywilson 2:de355b6fbd87 7 #define PI 3.14159
emilywilson 2:de355b6fbd87 8 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
emilywilson 2:de355b6fbd87 9
emilywilson 2:de355b6fbd87 10 class Line {
emilywilson 2:de355b6fbd87 11 public:
emilywilson 2:de355b6fbd87 12 int x;
emilywilson 2:de355b6fbd87 13 int y;
emilywilson 2:de355b6fbd87 14 };
emilywilson 2:de355b6fbd87 15
emilywilson 2:de355b6fbd87 16 uLCD_4DGL uLCD(p28,p27,p30);
emilywilson 2:de355b6fbd87 17 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
emilywilson 2:de355b6fbd87 18 Serial pc(USBTX, USBRX);
emilywilson 2:de355b6fbd87 19
emilywilson 2:de355b6fbd87 20 PinDetect pb(p16, PullUp);
emilywilson 2:de355b6fbd87 21
emilywilson 2:de355b6fbd87 22 int outer_radius = 15;
emilywilson 2:de355b6fbd87 23 int inner_radius = 10;
emilywilson 2:de355b6fbd87 24
emilywilson 2:de355b6fbd87 25 int center = (int)floor(127.0 / 2.0);
emilywilson 2:de355b6fbd87 26 int max_diff = 66536;
emilywilson 2:de355b6fbd87 27
emilywilson 2:de355b6fbd87 28 int radius = 10;
emilywilson 2:de355b6fbd87 29 int line_length = 20;
emilywilson 2:de355b6fbd87 30
emilywilson 2:de355b6fbd87 31 float getAttitude(float ax, float ay, float az, float mx, float my, float mz)
emilywilson 2:de355b6fbd87 32 {
emilywilson 2:de355b6fbd87 33 float roll = atan2(ay, az);
emilywilson 2:de355b6fbd87 34 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
emilywilson 2:de355b6fbd87 35 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
emilywilson 2:de355b6fbd87 36 mx = -mx;
emilywilson 2:de355b6fbd87 37 float heading;
emilywilson 2:de355b6fbd87 38 if (my == 0.0)
emilywilson 2:de355b6fbd87 39 heading = (mx < 0.0) ? 180.0 : 0.0;
emilywilson 2:de355b6fbd87 40 else
emilywilson 2:de355b6fbd87 41 heading = atan2(mx, my)*360.0/(2.0*PI);
emilywilson 2:de355b6fbd87 42 //pc.printf("heading atan=%f \n\r",heading);
emilywilson 2:de355b6fbd87 43 heading -= DECLINATION; //correct for geo location
emilywilson 2:de355b6fbd87 44 if(heading>180.0) heading = heading - 360.0;
emilywilson 2:de355b6fbd87 45 else if(heading<-180.0) heading = 360.0 + heading;
emilywilson 2:de355b6fbd87 46 else if(heading<0.0) heading = 360.0 + heading;
emilywilson 2:de355b6fbd87 47
emilywilson 2:de355b6fbd87 48
emilywilson 2:de355b6fbd87 49 // Convert everything from radians to degrees:
emilywilson 2:de355b6fbd87 50 //heading *= 180.0 / PI;
emilywilson 2:de355b6fbd87 51 pitch *= 180.0 / PI;
emilywilson 2:de355b6fbd87 52 roll *= 180.0 / PI;
emilywilson 2:de355b6fbd87 53
emilywilson 2:de355b6fbd87 54 // pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
emilywilson 2:de355b6fbd87 55 return heading;
emilywilson 2:de355b6fbd87 56 }
emilywilson 2:de355b6fbd87 57
emilywilson 2:de355b6fbd87 58 float compensated_heading(float ax, float ay, float az, float mx, float my, float mz) {
emilywilson 2:de355b6fbd87 59 float p_ax, p_ay, p_az, p_mx, p_my, p_mz;
emilywilson 2:de355b6fbd87 60 p_ax = ax; p_ay = ay; p_az = az;
emilywilson 2:de355b6fbd87 61 p_mx = mx; p_my = my; p_mz = mz;
emilywilson 2:de355b6fbd87 62
emilywilson 2:de355b6fbd87 63 float roll = atan2(p_ay, p_az);
emilywilson 2:de355b6fbd87 64 float iSin = sin(roll);
emilywilson 2:de355b6fbd87 65 float iCos = cos(roll);
emilywilson 2:de355b6fbd87 66
emilywilson 2:de355b6fbd87 67 float f_my = (int)((p_my * iCos) - (p_mz * iSin)) >> 15;
emilywilson 2:de355b6fbd87 68 p_mz = (int)((p_my * iSin) + (p_mz * iCos)) >> 15;
emilywilson 2:de355b6fbd87 69 p_az = (int)((p_ay * iSin) + (p_az * iCos)) >> 15;
emilywilson 2:de355b6fbd87 70
emilywilson 2:de355b6fbd87 71 float pitch = atan2(-p_ax, sqrt(p_ay * p_ay + p_az * p_az));
emilywilson 2:de355b6fbd87 72 iSin = sin(pitch);
emilywilson 2:de355b6fbd87 73 iCos = cos(pitch);
emilywilson 2:de355b6fbd87 74 if (iCos < 0) iCos = -iCos;
emilywilson 2:de355b6fbd87 75
emilywilson 2:de355b6fbd87 76 float f_mx = (int)((p_mx * iCos) + (p_mz * iSin)) >> 15;
emilywilson 2:de355b6fbd87 77
emilywilson 2:de355b6fbd87 78 float heading = atan2(-f_my, f_mx)*360 / (2*PI);
emilywilson 2:de355b6fbd87 79 heading -= DECLINATION; //correct for geo location
emilywilson 2:de355b6fbd87 80 if(heading>180.0) heading = heading - 360.0;
emilywilson 2:de355b6fbd87 81 else if(heading<-180.0) heading = 360.0 + heading;
emilywilson 2:de355b6fbd87 82 else if(heading<0.0) heading = 360.0 + heading;
emilywilson 2:de355b6fbd87 83
emilywilson 2:de355b6fbd87 84 return heading;
emilywilson 2:de355b6fbd87 85 }
emilywilson 2:de355b6fbd87 86
emilywilson 2:de355b6fbd87 87 int calc_x(float heading, int line_length) {
emilywilson 2:de355b6fbd87 88 float rad = (PI*heading/180);
emilywilson 2:de355b6fbd87 89
emilywilson 2:de355b6fbd87 90 return (int)(line_length * cos(rad));
emilywilson 2:de355b6fbd87 91 }
emilywilson 2:de355b6fbd87 92
emilywilson 2:de355b6fbd87 93 int calc_y(float heading, int line_length) {
emilywilson 2:de355b6fbd87 94 float rad = (PI*heading/180);
emilywilson 2:de355b6fbd87 95
emilywilson 2:de355b6fbd87 96 return (int)(line_length * sin(rad));
emilywilson 2:de355b6fbd87 97 }
emilywilson 0:90df82d4732d 98
emilywilson 0:90df82d4732d 99 int run_part3() {
emilywilson 2:de355b6fbd87 100 IMU.begin();
emilywilson 2:de355b6fbd87 101 if (!IMU.begin()) {
emilywilson 2:de355b6fbd87 102 pc.printf("Failed to communicate with LSM9DS1.\n");
emilywilson 2:de355b6fbd87 103 }
emilywilson 2:de355b6fbd87 104 IMU.calibrate(1);
emilywilson 2:de355b6fbd87 105 IMU.calibrateMag(0);
emilywilson 2:de355b6fbd87 106
emilywilson 2:de355b6fbd87 107 pc.printf("finished imu begin");
emilywilson 0:90df82d4732d 108
emilywilson 0:90df82d4732d 109 int center = (int)floor(127.0 / 2.0);
emilywilson 2:de355b6fbd87 110 int curr_x = center;
emilywilson 2:de355b6fbd87 111 int curr_y = center;
emilywilson 2:de355b6fbd87 112
emilywilson 2:de355b6fbd87 113 while(1) {
emilywilson 2:de355b6fbd87 114 while (!IMU.accelAvailable());
emilywilson 2:de355b6fbd87 115 IMU.readAccel();
emilywilson 2:de355b6fbd87 116
emilywilson 2:de355b6fbd87 117 uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
emilywilson 2:de355b6fbd87 118 uLCD.circle(center, center, outer_radius, WHITE);
emilywilson 2:de355b6fbd87 119
emilywilson 2:de355b6fbd87 120 int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
emilywilson 2:de355b6fbd87 121 int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
emilywilson 2:de355b6fbd87 122
emilywilson 2:de355b6fbd87 123 curr_x = center + y_diff;
emilywilson 2:de355b6fbd87 124 curr_y = center - x_diff;
emilywilson 2:de355b6fbd87 125
emilywilson 2:de355b6fbd87 126 uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
emilywilson 2:de355b6fbd87 127 wait(0.01);
emilywilson 2:de355b6fbd87 128 }
emilywilson 2:de355b6fbd87 129 }
emilywilson 2:de355b6fbd87 130
emilywilson 2:de355b6fbd87 131 int run_part3_EC() {
emilywilson 2:de355b6fbd87 132 IMU.begin();
emilywilson 2:de355b6fbd87 133 IMU.calibrate(1);
emilywilson 2:de355b6fbd87 134 IMU.calibrateMag(0);
emilywilson 2:de355b6fbd87 135
emilywilson 2:de355b6fbd87 136 uLCD.background_color(BLACK);
emilywilson 2:de355b6fbd87 137
emilywilson 2:de355b6fbd87 138 Line curr_line;
emilywilson 2:de355b6fbd87 139 curr_line.x = center;
emilywilson 2:de355b6fbd87 140 curr_line.y = center + line_length;
emilywilson 2:de355b6fbd87 141
emilywilson 2:de355b6fbd87 142 while (1) {
emilywilson 2:de355b6fbd87 143 uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
emilywilson 2:de355b6fbd87 144 uLCD.filled_circle(center, center, radius, BLACK);
emilywilson 2:de355b6fbd87 145 uLCD.circle(center, center, radius, WHITE);
emilywilson 2:de355b6fbd87 146
emilywilson 2:de355b6fbd87 147 IMU.readMag();
emilywilson 2:de355b6fbd87 148 IMU.readAccel();
emilywilson 2:de355b6fbd87 149
emilywilson 2:de355b6fbd87 150 float heading = getAttitude(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
emilywilson 2:de355b6fbd87 151
emilywilson 2:de355b6fbd87 152 curr_line.x = center + calc_x(heading, line_length);
emilywilson 2:de355b6fbd87 153 curr_line.y = center + calc_y(heading, line_length);
emilywilson 2:de355b6fbd87 154
emilywilson 2:de355b6fbd87 155 uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
emilywilson 2:de355b6fbd87 156
emilywilson 2:de355b6fbd87 157 char buffer[50];
emilywilson 2:de355b6fbd87 158 sprintf(buffer, "Heading: %f", heading);
emilywilson 2:de355b6fbd87 159
emilywilson 2:de355b6fbd87 160 uLCD.text_string(buffer, 100, 10, 12, WHITE);
emilywilson 2:de355b6fbd87 161
emilywilson 2:de355b6fbd87 162 wait(0.1);
emilywilson 2:de355b6fbd87 163 }
emilywilson 2:de355b6fbd87 164 }
emilywilson 2:de355b6fbd87 165
emilywilson 2:de355b6fbd87 166 int run_pb_EC() {
emilywilson 2:de355b6fbd87 167 IMU.begin();
emilywilson 2:de355b6fbd87 168 IMU.calibrate(1);
emilywilson 2:de355b6fbd87 169 IMU.calibrateMag(0);
emilywilson 0:90df82d4732d 170
emilywilson 0:90df82d4732d 171 int curr_x = center;
emilywilson 0:90df82d4732d 172 int curr_y = center;
emilywilson 0:90df82d4732d 173
emilywilson 1:6d8f645530b8 174 uLCD.background_color(BLACK);
emilywilson 1:6d8f645530b8 175
emilywilson 2:de355b6fbd87 176 Line curr_line;
emilywilson 2:de355b6fbd87 177 curr_line.x = center;
emilywilson 2:de355b6fbd87 178 curr_line.y = center + line_length;
emilywilson 1:6d8f645530b8 179
emilywilson 2:de355b6fbd87 180 int prev_pb = pb;
emilywilson 2:de355b6fbd87 181 int mode = 0;
emilywilson 1:6d8f645530b8 182
emilywilson 1:6d8f645530b8 183 while (1) {
emilywilson 2:de355b6fbd87 184 if (pb ^ prev_pb) {
emilywilson 2:de355b6fbd87 185 mode = !mode;
emilywilson 2:de355b6fbd87 186 }
emilywilson 1:6d8f645530b8 187
emilywilson 2:de355b6fbd87 188 if (mode == 0) {
emilywilson 2:de355b6fbd87 189 uLCD.line(center, center, curr_line.x, curr_line.y, BLACK);
emilywilson 2:de355b6fbd87 190 uLCD.filled_circle(center, center, radius, BLACK);
emilywilson 2:de355b6fbd87 191 uLCD.circle(center, center, radius, WHITE);
emilywilson 2:de355b6fbd87 192
emilywilson 2:de355b6fbd87 193 IMU.readMag();
emilywilson 2:de355b6fbd87 194 IMU.readAccel();
emilywilson 2:de355b6fbd87 195
emilywilson 2:de355b6fbd87 196 float heading = compensated_heading(IMU.ax, IMU.ay, IMU.az, IMU.mx, IMU.my, IMU.mz);
emilywilson 2:de355b6fbd87 197
emilywilson 2:de355b6fbd87 198 curr_line.x = center + calc_x(heading, line_length);
emilywilson 2:de355b6fbd87 199 curr_line.y = center + calc_y(heading, line_length);
emilywilson 2:de355b6fbd87 200
emilywilson 2:de355b6fbd87 201 uLCD.line(center, center, curr_line.x, curr_line.y, WHITE);
emilywilson 2:de355b6fbd87 202
emilywilson 2:de355b6fbd87 203 char buffer[50];
emilywilson 2:de355b6fbd87 204 sprintf(buffer, "Heading: %f", heading);
emilywilson 2:de355b6fbd87 205
emilywilson 2:de355b6fbd87 206 uLCD.text_string(buffer, 100, 10, 12, WHITE);
emilywilson 2:de355b6fbd87 207
emilywilson 2:de355b6fbd87 208 wait(0.1);
emilywilson 2:de355b6fbd87 209 } else {
emilywilson 2:de355b6fbd87 210 while (!IMU.accelAvailable());
emilywilson 2:de355b6fbd87 211 IMU.readAccel();
emilywilson 2:de355b6fbd87 212
emilywilson 2:de355b6fbd87 213 uLCD.filled_circle(curr_x, curr_y, inner_radius, BLACK);
emilywilson 2:de355b6fbd87 214 uLCD.circle(center, center, outer_radius, WHITE);
emilywilson 2:de355b6fbd87 215
emilywilson 2:de355b6fbd87 216 int y_diff = IMU.calcAccel(IMU.ay) * 3.0;
emilywilson 2:de355b6fbd87 217 int x_diff = IMU.calcAccel(IMU.ax) * 3.0;
emilywilson 2:de355b6fbd87 218
emilywilson 2:de355b6fbd87 219 curr_x = center + y_diff;
emilywilson 2:de355b6fbd87 220 curr_y = center - x_diff;
emilywilson 2:de355b6fbd87 221
emilywilson 2:de355b6fbd87 222 uLCD.filled_circle(curr_x, curr_y, inner_radius, WHITE);
emilywilson 2:de355b6fbd87 223 wait(0.01);
emilywilson 2:de355b6fbd87 224 }
emilywilson 1:6d8f645530b8 225 }
emilywilson 1:6d8f645530b8 226 }
emilywilson 1:6d8f645530b8 227
emilywilson 1:6d8f645530b8 228 int run_time_EC() {
emilywilson 1:6d8f645530b8 229 set_time(1580736096);
emilywilson 1:6d8f645530b8 230
emilywilson 1:6d8f645530b8 231 int center = (int)floor(127.0 / 2.0);
emilywilson 1:6d8f645530b8 232
emilywilson 1:6d8f645530b8 233 while (1) {
emilywilson 1:6d8f645530b8 234 time_t seconds = time(NULL);
emilywilson 1:6d8f645530b8 235
emilywilson 1:6d8f645530b8 236 char buffer[50];
emilywilson 1:6d8f645530b8 237 sprintf(buffer, "Time: %s", ctime(&seconds));
emilywilson 1:6d8f645530b8 238
emilywilson 2:de355b6fbd87 239 uLCD.text_string(buffer, center, center, 10, RED);
emilywilson 2:de355b6fbd87 240
emilywilson 2:de355b6fbd87 241 wait(0.2);
emilywilson 2:de355b6fbd87 242 uLCD.rectangle(0, 0, 127, 127, BLACK);
emilywilson 0:90df82d4732d 243 }
emilywilson 0:90df82d4732d 244 }