Mark Schwarzer / Mbed 2 deprecated Project_OCE360

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

main.cpp@11:b62f06b3e398, 2020-12-06 (annotated)

Committer:
markschwarzer
Date:
Sun Dec 06 21:22:00 2020 +0000
Revision:
11:b62f06b3e398
Parent:
10:02584efdaa39
Child:
12:2f1e8b205d0f
updated data outputs under the "log functions" section of the code, and also tried to change the pulse widths to achieve depth holding.

Who changed what in which revision?

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