Alek Boving / Mbed 2 deprecated project1

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 LSM9DS1project SDFileSystemproject SCI_SENSORproject

main.cpp@1:90d5de35324c, 2020-12-03 (annotated)

Committer:
alekboving
Date:
Thu Dec 03 14:06:20 2020 +0000
Revision:
1:90d5de35324c
Parent:
0:47a98d724c30 
Initial commit w some test code

Who changed what in which revision?

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