Madeline Kistler / Mbed 2 deprecated Final_Project

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MS5837_final LSM9DS1_final SDFileSystem_final SCI_SENSOR_final

main.cpp@2:ba90f3e05798, 2020-12-01 (annotated)

Committer:
mkistler
Date:
Tue Dec 01 14:46:30 2020 +0000
Revision:
2:ba90f3e05798
Parent:
1:88ebad269e60
Child:
3:8975c76293be
Log_data: Compiles and should work? Maddy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkistler 0:187a5ea5239b 1 /*
mkistler 0:187a5ea5239b 2 Author Mingxi Zhou
mkistler 0:187a5ea5239b 3 OCE360 underwater float template program
mkistler 0:187a5ea5239b 4 */
mkistler 0:187a5ea5239b 5 #include "mbed.h"
mkistler 0:187a5ea5239b 6 #include "LSM9DS1.h" //IMU library
mkistler 0:187a5ea5239b 7 #include "MS5837.h" //pressure sensor library
mkistler 0:187a5ea5239b 8 #include "SCI_SENSOR.h" //science sensor
mkistler 0:187a5ea5239b 9 #include "SDFileSystem.h" // SD card
mkistler 0:187a5ea5239b 10
mkistler 0:187a5ea5239b 11 DigitalOut myled(LED1);
mkistler 0:187a5ea5239b 12 Serial pc(USBTX, USBRX); //initial serial
mkistler 0:187a5ea5239b 13 Serial BLE(p13,p14); //Bluetooth
mkistler 0:187a5ea5239b 14 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); //initial IMU
mkistler 0:187a5ea5239b 15 LM19 temp(p19);
mkistler 0:187a5ea5239b 16 PhotoCell light(p20);
mkistler 0:187a5ea5239b 17 MS5837 p_sensor(p9, p10, ms5837_addr_no_CS); //pressure sensor
mkistler 0:187a5ea5239b 18 PwmOut thruster(p21); //set PWM pin //max 1.3ms min 1.1ms
mkistler 0:187a5ea5239b 19 PwmOut thruster2(p22); //set PWM pin
mkistler 0:187a5ea5239b 20 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
mkistler 0:187a5ea5239b 21
mkistler 2:ba90f3e05798 22 float pw;
mkistler 2:ba90f3e05798 23
mkistler 0:187a5ea5239b 24 //global ticker
mkistler 0:187a5ea5239b 25 Ticker log_ticker;
mkistler 0:187a5ea5239b 26 Ticker imu_ticker;
mkistler 0:187a5ea5239b 27 // global timer
mkistler 0:187a5ea5239b 28 Timer t;
mkistler 0:187a5ea5239b 29 //MS5837 p_sensor(p9, p10, ms5837_addr_no_CS);
mkistler 0:187a5ea5239b 30 ///File
mkistler 0:187a5ea5239b 31 FILE *fp;
mkistler 0:187a5ea5239b 32 char fname[100];
mkistler 0:187a5ea5239b 33 float PI = 3.14159265358979323846f;
mkistler 0:187a5ea5239b 34
mkistler 0:187a5ea5239b 35 //float operation parameters
mkistler 0:187a5ea5239b 36 float target_depth=0; //global target depth default 0
mkistler 0:187a5ea5239b 37 int yo_num=0; //global yo_num default 0
mkistler 0:187a5ea5239b 38 float thrust_on_time=0; //global thrust_on time default 0
mkistler 0:187a5ea5239b 39 float accel[3], mag[3], gyro[3], euler[3]; //global IMU data
mkistler 0:187a5ea5239b 40
mkistler 0:187a5ea5239b 41 //functions
mkistler 0:187a5ea5239b 42 void welcome();
mkistler 0:187a5ea5239b 43 void log_data();
mkistler 0:187a5ea5239b 44 //IMU related
mkistler 0:187a5ea5239b 45 void IMU_update(); //update IMU related varibles. we use imu_ticker to call this function
mkistler 0:187a5ea5239b 46 void mag_correction(float mx, float my, float mz, float mag_c[3]); //raw mag -> mag[3], mag_c[3] calibrated
mkistler 0:187a5ea5239b 47 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]); //pose estimation function
mkistler 0:187a5ea5239b 48 //Control related functions
mkistler 0:187a5ea5239b 49 void thrust_on(float pw, float on_time); //input is pulse width
mkistler 0:187a5ea5239b 50
mkistler 0:187a5ea5239b 51 //-------------Main functions-----------------------------------------------------------------------------------------
mkistler 0:187a5ea5239b 52 int main()
mkistler 0:187a5ea5239b 53 {
mkistler 0:187a5ea5239b 54 //-----Initialization realted code-------//
mkistler 1:88ebad269e60 55 //inital set the thruster esc to 1ms duty cycle
mkistler 0:187a5ea5239b 56 thruster.period(0.002); // 2 ms period
mkistler 0:187a5ea5239b 57 thruster.pulsewidth(1.0/1000.000); /////IMU initial and begin
mkistler 0:187a5ea5239b 58 thruster2.period(0.002); // 2 ms period
mkistler 0:187a5ea5239b 59 thruster2.pulsewidth(1.0/1000.000); /////IMU initial and begin
mkistler 0:187a5ea5239b 60 IMU.begin();
mkistler 0:187a5ea5239b 61 IMU.calibrate(true);
mkistler 0:187a5ea5239b 62 myled=1;
mkistler 0:187a5ea5239b 63 //initialize pressure sensor
mkistler 0:187a5ea5239b 64 pc.printf("setting the pressure sensor\r\n");
mkistler 0:187a5ea5239b 65 p_sensor.MS5837Reset();
mkistler 0:187a5ea5239b 66 p_sensor.MS5837Init();
mkistler 0:187a5ea5239b 67 pc.printf("settting the tickers\r\n");
mkistler 0:187a5ea5239b 68 t.start();
mkistler 0:187a5ea5239b 69 myled=0;
mkistler 0:187a5ea5239b 70 welcome();
mkistler 0:187a5ea5239b 71 //-----setup ticker-------//
mkistler 0:187a5ea5239b 72 //setup ticker to separate log and IMU data update.
mkistler 0:187a5ea5239b 73 //so we could have all our control code in the while loop
mkistler 0:187a5ea5239b 74 // //log at 2 Hz
mkistler 0:187a5ea5239b 75 imu_ticker.attach(&IMU_update,0.1); //10Hz
mkistler 0:187a5ea5239b 76 log_ticker.attach(&log_data,0.5);
mkistler 0:187a5ea5239b 77 wait(1);
mkistler 1:88ebad269e60 78 while(1) {
mkistler 1:88ebad269e60 79 // put your main control code here
mkistler 0:187a5ea5239b 80 }
mkistler 0:187a5ea5239b 81
mkistler 0:187a5ea5239b 82 }
mkistler 0:187a5ea5239b 83
mkistler 0:187a5ea5239b 84 //-------------Customized functions---------------------------------------------//----------------------------------------
mkistler 0:187a5ea5239b 85 ///-----------Welcome menu---------------------///
mkistler 0:187a5ea5239b 86 void welcome()
mkistler 0:187a5ea5239b 87 {
mkistler 1:88ebad269e60 88 char buffer[100]= {0};
mkistler 0:187a5ea5239b 89 int flag=1;
mkistler 0:187a5ea5239b 90 //Flush the port
mkistler 1:88ebad269e60 91 while(BLE.readable()) {
mkistler 1:88ebad269e60 92 BLE.getc();
mkistler 0:187a5ea5239b 93 }
mkistler 1:88ebad269e60 94 while(flag) {
mkistler 0:187a5ea5239b 95 BLE.printf("### I am alive\r\n");
mkistler 0:187a5ea5239b 96 BLE.printf("### Please enter the log file name you want\r\n");
mkistler 1:88ebad269e60 97 if(BLE.readable()) {
mkistler 0:187a5ea5239b 98 BLE.scanf("%s",buffer);
mkistler 0:187a5ea5239b 99 sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name
mkistler 0:187a5ea5239b 100
mkistler 0:187a5ea5239b 101 flag = 0; //set the flag to 0 to break the while
mkistler 0:187a5ea5239b 102 }
mkistler 0:187a5ea5239b 103 myled=!myled;
mkistler 0:187a5ea5239b 104 wait(1);
mkistler 0:187a5ea5239b 105 }
mkistler 0:187a5ea5239b 106 //print name
mkistler 0:187a5ea5239b 107 BLE.printf("### name received\r\n");
mkistler 0:187a5ea5239b 108 BLE.printf("### file name and directory is: \r\n %s\r\n",fname); //file name and location
mkistler 0:187a5ea5239b 109 //open file test
mkistler 0:187a5ea5239b 110 mkdir("/sd/mydir",0777); //keep 0777, this is magic #
mkistler 0:187a5ea5239b 111 fp = fopen(fname, "a");
mkistler 1:88ebad269e60 112 if(fp == NULL) {
mkistler 0:187a5ea5239b 113 BLE.printf("Could not open file for write\n");
mkistler 1:88ebad269e60 114 } else {
mkistler 0:187a5ea5239b 115 BLE.printf("##file open good \n"); //open file and tell if open
mkistler 0:187a5ea5239b 116 fprintf(fp, "Hello\r\n");
mkistler 0:187a5ea5239b 117 fclose(fp);
mkistler 0:187a5ea5239b 118 }
mkistler 0:187a5ea5239b 119
mkistler 0:187a5ea5239b 120 BLE.printf("### The main program will start in 10 seconds\r\n");
mkistler 0:187a5ea5239b 121 wait(5);
mkistler 0:187a5ea5239b 122 }
mkistler 0:187a5ea5239b 123
mkistler 0:187a5ea5239b 124 ///-----------log functions---------------------///
mkistler 0:187a5ea5239b 125 void log_data()
mkistler 0:187a5ea5239b 126 {
mkistler 1:88ebad269e60 127 IMU_update();
mkistler 1:88ebad269e60 128 fp = fopen(fname, "a");
mkistler 1:88ebad269e60 129
mkistler 1:88ebad269e60 130 if(fp == NULL) { // If the file didn't open we'd want to know.
mkistler 1:88ebad269e60 131 error("Could not open file for write\n");
mkistler 1:88ebad269e60 132
mkistler 1:88ebad269e60 133 }
mkistler 2:ba90f3e05798 134 fprintf(fp, "$NEMA,1,15,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f;", t.read(), //log system time t.read()
mkistler 1:88ebad269e60 135 p_sensor.depth(), p_sensor.MS5837_Temperature(), // log pressure sensor data.
mkistler 1:88ebad269e60 136 pw, // log pulse width
mkistler 1:88ebad269e60 137 IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), // log imu data, log sciene data
mkistler 1:88ebad269e60 138 IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz),
mkistler 1:88ebad269e60 139 euler[0],euler[1],euler[2], //IMU sensor
mkistler 1:88ebad269e60 140 temp.temp(), light.light()); //science sensor: temp.temp(), light.light()
mkistler 1:88ebad269e60 141
mkistler 2:ba90f3e05798 142 fclose(fp);
mkistler 0:187a5ea5239b 143
mkistler 0:187a5ea5239b 144 }
mkistler 0:187a5ea5239b 145
mkistler 0:187a5ea5239b 146 ///-----------IMU related functions---------------------///
mkistler 0:187a5ea5239b 147 void IMU_update()
mkistler 0:187a5ea5239b 148 {
mkistler 0:187a5ea5239b 149 IMU.readMag();
mkistler 0:187a5ea5239b 150 IMU.readGyro();
mkistler 0:187a5ea5239b 151 IMU.readAccel();
mkistler 0:187a5ea5239b 152 accel[0] = IMU.calcAccel(IMU.ax);
mkistler 0:187a5ea5239b 153 accel[1] = IMU.calcAccel(IMU.ay);
mkistler 0:187a5ea5239b 154 accel[2] = -IMU.calcAccel(IMU.az);
mkistler 0:187a5ea5239b 155 gyro[0] = IMU.calcGyro(IMU.gx);
mkistler 0:187a5ea5239b 156 gyro[1] = IMU.calcGyro(IMU.gy);
mkistler 0:187a5ea5239b 157 gyro[2] = -IMU.calcGyro(IMU.gz);
mkistler 0:187a5ea5239b 158 mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag); //mag correction
mkistler 0:187a5ea5239b 159 mag[2] = - mag[2];
mkistler 0:187a5ea5239b 160 pose_estimate(euler, accel, gyro, mag); //pose update
mkistler 0:187a5ea5239b 161 }
mkistler 0:187a5ea5239b 162
mkistler 0:187a5ea5239b 163 void mag_correction(float mx, float my, float mz, float mag_c[3])
mkistler 0:187a5ea5239b 164 {
mkistler 0:187a5ea5239b 165 float bias[3] = {0.0793,0.0357,0.2333};
mkistler 0:187a5ea5239b 166 float scale[3][3] = {{1.0070, 0.0705, 0.0368},
mkistler 1:88ebad269e60 167 {0.0705, 1.0807, 0.0265},
mkistler 1:88ebad269e60 168 {0.0368, 0.0265, 0.9250}
mkistler 1:88ebad269e60 169 };
mkistler 0:187a5ea5239b 170 //mag_c = (mag-bias)*scale
mkistler 0:187a5ea5239b 171
mkistler 0:187a5ea5239b 172 mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
mkistler 0:187a5ea5239b 173 mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
mkistler 0:187a5ea5239b 174 mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
mkistler 0:187a5ea5239b 175 }
mkistler 0:187a5ea5239b 176
mkistler 0:187a5ea5239b 177 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]) //pose estimation function
mkistler 0:187a5ea5239b 178 {
mkistler 1:88ebad269e60 179 euler[0] = atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
mkistler 1:88ebad269e60 180 euler[1] = - atan2( -accel[0],( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
mkistler 1:88ebad269e60 181 float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
mkistler 1:88ebad269e60 182 float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
mkistler 1:88ebad269e60 183 + (mag[2] * cos(euler[0]) * sin(euler[1]));
mkistler 1:88ebad269e60 184 euler[2] = atan2(Yh, Xh);
mkistler 1:88ebad269e60 185 //convert into degrees
mkistler 1:88ebad269e60 186 euler[0] *= 180.0f / PI;
mkistler 1:88ebad269e60 187 euler[1] *= 180.0f / PI;
mkistler 1:88ebad269e60 188 euler[2] *= 180.0f / PI;
mkistler 1:88ebad269e60 189 //wrap the values to be within 0 to 360.
mkistler 1:88ebad269e60 190 for (int i=0; i<3; i++) {
mkistler 1:88ebad269e60 191 if(euler[i]<=0) {
mkistler 1:88ebad269e60 192 euler[i]=euler[i]+360;
mkistler 0:187a5ea5239b 193 }
mkistler 1:88ebad269e60 194 if(euler[i]>360) {
mkistler 1:88ebad269e60 195 euler[i]=euler[i]-360;
mkistler 1:88ebad269e60 196 }
mkistler 1:88ebad269e60 197 }
mkistler 0:187a5ea5239b 198
mkistler 0:187a5ea5239b 199 }
mkistler 0:187a5ea5239b 200
mkistler 0:187a5ea5239b 201 ///-----------Control related functions---------------------///
mkistler 0:187a5ea5239b 202 ////Thruster on control, pw->pulse width in milli-second//
mkistler 0:187a5ea5239b 203 //// pw range between 1 to 1.5//
mkistler 0:187a5ea5239b 204 //// on_time-> thruster on time.
mkistler 0:187a5ea5239b 205 void thrust_on(float pw, float on_time) //input is pulse width
mkistler 0:187a5ea5239b 206 {
mkistler 0:187a5ea5239b 207 float pw_max=2.0;
mkistler 1:88ebad269e60 208 if(pw>pw_max) {
mkistler 0:187a5ea5239b 209 pw=pw_max; //hard limitation
mkistler 0:187a5ea5239b 210 }
mkistler 0:187a5ea5239b 211 Timer tt;
mkistler 0:187a5ea5239b 212 tt.reset();
mkistler 0:187a5ea5239b 213 tt.start();
mkistler 0:187a5ea5239b 214 // lets set the pulse width
mkistler 0:187a5ea5239b 215 //thruster.period(20.0/1000.00); // 20 ms period
mkistler 0:187a5ea5239b 216 thruster.pulsewidth(pw/1000.00);
mkistler 0:187a5ea5239b 217 thruster2.pulsewidth(pw/1000.00);
mkistler 0:187a5ea5239b 218 //PWM will be kept until time out
mkistler 1:88ebad269e60 219 while(tt.read()<=on_time) {
mkistler 0:187a5ea5239b 220 }
mkistler 0:187a5ea5239b 221 //stop the timer
mkistler 0:187a5ea5239b 222 tt.stop();
mkistler 0:187a5ea5239b 223 //turn off the thruster
mkistler 0:187a5ea5239b 224 thruster.pulsewidth(1.0/1000.00);
mkistler 0:187a5ea5239b 225 thruster2.pulsewidth(1.0/1000.00);
mkistler 0:187a5ea5239b 226
mkistler 0:187a5ea5239b 227 }