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@15:6cb9a9ede7fe, 2020-12-08 (annotated)

Committer:
markschwarzer
Date:
Tue Dec 08 14:25:55 2020 +0000
Revision:
15:6cb9a9ede7fe
Parent:
12:2f1e8b205d0f 
Moved timer ttt

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
jrschaedler 12:2f1e8b205d0f 16 LM19 t_sensor(p19);
jrschaedler 12:2f1e8b205d0f 17 PhotoCell photo(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 15:6cb9a9ede7fe 35 float pw;
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 15:6cb9a9ede7fe 79 ttt.start();
markschwarzer 0:852fb667f2d6 80 while(1)
markschwarzer 1:558ab0afbbc5 81 {
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 }
markschwarzer 15:6cb9a9ede7fe 96
markschwarzer 15:6cb9a9ede7fe 97 if(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 15:6cb9a9ede7fe 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 15:6cb9a9ede7fe 178 fprintf(fp, "$NMEA, 3,15, %f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f;\r\n",t.read(),p_sensor.depth(),p_sensor.MS5837_Temperature(),pw,accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2],euler[0],euler[1],euler[2],t_sensor.temp(),photo.light());
markschwarzer 2:5ee1cbcf8e04 179 fclose(fp);
markschwarzer 11:b62f06b3e398 180
markschwarzer 0:852fb667f2d6 181 // log pulse width
markschwarzer 0:852fb667f2d6 182 // log pressure sensor data.
markschwarzer 0:852fb667f2d6 183
markschwarzer 0:852fb667f2d6 184 }
markschwarzer 0:852fb667f2d6 185
markschwarzer 0:852fb667f2d6 186 ///-----------IMU related functions---------------------///
markschwarzer 0:852fb667f2d6 187 void IMU_update()
markschwarzer 0:852fb667f2d6 188 {
markschwarzer 0:852fb667f2d6 189 IMU.readMag();
markschwarzer 0:852fb667f2d6 190 IMU.readGyro();
markschwarzer 0:852fb667f2d6 191 IMU.readAccel();
markschwarzer 0:852fb667f2d6 192 accel[0] = IMU.calcAccel(IMU.ax);
markschwarzer 0:852fb667f2d6 193 accel[1] = IMU.calcAccel(IMU.ay);
markschwarzer 0:852fb667f2d6 194 accel[2] = -IMU.calcAccel(IMU.az);
markschwarzer 0:852fb667f2d6 195 gyro[0] = IMU.calcGyro(IMU.gx);
markschwarzer 0:852fb667f2d6 196 gyro[1] = IMU.calcGyro(IMU.gy);
markschwarzer 0:852fb667f2d6 197 gyro[2] = -IMU.calcGyro(IMU.gz);
markschwarzer 0:852fb667f2d6 198 mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag); //mag correction
markschwarzer 0:852fb667f2d6 199 mag[2] = - mag[2];
markschwarzer 0:852fb667f2d6 200 pose_estimate(euler, accel, gyro, mag); //pose update
markschwarzer 0:852fb667f2d6 201 }
markschwarzer 0:852fb667f2d6 202
markschwarzer 0:852fb667f2d6 203 void mag_correction(float mx, float my, float mz, float mag_c[3])
markschwarzer 0:852fb667f2d6 204 {
markschwarzer 0:852fb667f2d6 205 float bias[3] = {0.0793,0.0357,0.2333};
markschwarzer 0:852fb667f2d6 206 float scale[3][3] = {{1.0070, 0.0705, 0.0368},
markschwarzer 0:852fb667f2d6 207 {0.0705, 1.0807, 0.0265},
markschwarzer 0:852fb667f2d6 208 {0.0368, 0.0265, 0.9250}};
markschwarzer 0:852fb667f2d6 209 //mag_c = (mag-bias)*scale
markschwarzer 0:852fb667f2d6 210
markschwarzer 0:852fb667f2d6 211 mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
markschwarzer 0:852fb667f2d6 212 mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
markschwarzer 0:852fb667f2d6 213 mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
markschwarzer 0:852fb667f2d6 214 }
markschwarzer 0:852fb667f2d6 215
markschwarzer 0:852fb667f2d6 216 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]) //pose estimation function
markschwarzer 0:852fb667f2d6 217 {
markschwarzer 0:852fb667f2d6 218 euler[0] = atan2 (accel[1] , accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
markschwarzer 0:852fb667f2d6 219 euler[1] = - atan2( -accel[0] ,( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
markschwarzer 0:852fb667f2d6 220 float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
markschwarzer 0:852fb667f2d6 221 float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
markschwarzer 0:852fb667f2d6 222 + (mag[2] * cos(euler[0]) * sin(euler[1]));
markschwarzer 0:852fb667f2d6 223 euler[2] = atan2(Yh, Xh);
markschwarzer 0:852fb667f2d6 224 //convert into degrees
markschwarzer 0:852fb667f2d6 225 euler[0] *= 180.0f / PI;
markschwarzer 0:852fb667f2d6 226 euler[1] *= 180.0f / PI;
markschwarzer 0:852fb667f2d6 227 euler[2] *= 180.0f / PI;
markschwarzer 0:852fb667f2d6 228 //wrap the values to be within 0 to 360.
markschwarzer 0:852fb667f2d6 229 for (int i=0;i<3;i++)
markschwarzer 0:852fb667f2d6 230 {
markschwarzer 0:852fb667f2d6 231 if(euler[i]<=0)
markschwarzer 0:852fb667f2d6 232 {
markschwarzer 0:852fb667f2d6 233 euler[i]=euler[i]+360;
markschwarzer 0:852fb667f2d6 234 }
markschwarzer 0:852fb667f2d6 235 if(euler[i]>360)
markschwarzer 0:852fb667f2d6 236 {
markschwarzer 0:852fb667f2d6 237 euler[i]=euler[i]-360;
markschwarzer 0:852fb667f2d6 238 }
markschwarzer 0:852fb667f2d6 239 }
markschwarzer 0:852fb667f2d6 240
markschwarzer 0:852fb667f2d6 241 }
markschwarzer 0:852fb667f2d6 242
markschwarzer 0:852fb667f2d6 243 ///-----------Control related functions---------------------///
markschwarzer 0:852fb667f2d6 244 ////Thruster on control, pw->pulse width in milli-second//
markschwarzer 0:852fb667f2d6 245 //// pw range between 1 to 1.5//
markschwarzer 0:852fb667f2d6 246 //// on_time-> thruster on time.
markschwarzer 15:6cb9a9ede7fe 247 void thrust_on(float on_time) //input is pulse width
markschwarzer 0:852fb667f2d6 248 {
markschwarzer 0:852fb667f2d6 249 float pw_max=2.0;
markschwarzer 0:852fb667f2d6 250 if(pw>pw_max)
markschwarzer 0:852fb667f2d6 251 {
markschwarzer 0:852fb667f2d6 252 pw=pw_max; //hard limitation
markschwarzer 0:852fb667f2d6 253 }
markschwarzer 0:852fb667f2d6 254 Timer tt;
markschwarzer 0:852fb667f2d6 255 tt.reset();
markschwarzer 0:852fb667f2d6 256 tt.start();
markschwarzer 0:852fb667f2d6 257 // lets set the pulse width
markschwarzer 4:574998b40bde 258 thruster.period(20.0/1000.00); // 20 ms period
markschwarzer 0:852fb667f2d6 259 thruster.pulsewidth(pw/1000.00);
markschwarzer 0:852fb667f2d6 260 thruster2.pulsewidth(pw/1000.00);
markschwarzer 0:852fb667f2d6 261 //PWM will be kept until time out
markschwarzer 0:852fb667f2d6 262 while(tt.read()<=on_time)
markschwarzer 0:852fb667f2d6 263 {
markschwarzer 0:852fb667f2d6 264 }
markschwarzer 0:852fb667f2d6 265 //stop the timer
markschwarzer 0:852fb667f2d6 266 tt.stop();
markschwarzer 0:852fb667f2d6 267 //turn off the thruster
markschwarzer 0:852fb667f2d6 268 thruster.pulsewidth(1.0/1000.00);
markschwarzer 0:852fb667f2d6 269 thruster2.pulsewidth(1.0/1000.00);
markschwarzer 0:852fb667f2d6 270
markschwarzer 0:852fb667f2d6 271 }