Revision 1:90d5de35324c, committed 2020-12-03

Comitter:

alekboving

Date:

Thu Dec 03 14:06:20 2020 +0000

Parent:

0:47a98d724c30

Commit message:

Initial commit w some test code

Changed in this revision

--- a/LSM9DS1.lib	Tue Dec 01 15:02:30 2020 +0000
+++ b/LSM9DS1.lib	Thu Dec 03 14:06:20 2020 +0000
@@ -1,1 +1,1 @@
-LSM9DS1#35dccf1ddb1c
+https://os.mbed.com/users/alekboving/code/LSM9DS1project/#35dccf1ddb1c

--- a/MS5837.lib	Tue Dec 01 15:02:30 2020 +0000
+++ b/MS5837.lib	Thu Dec 03 14:06:20 2020 +0000
@@ -1,1 +1,1 @@
-MS5837#dd665d08f973
+https://os.mbed.com/users/alekboving/code/MS5837/#dd665d08f973

--- a/SCI_SENSOR.lib	Tue Dec 01 15:02:30 2020 +0000
+++ b/SCI_SENSOR.lib	Thu Dec 03 14:06:20 2020 +0000
@@ -1,1 +1,1 @@
-SCI_SENSOR#9bd6e618dff4
+https://os.mbed.com/users/alekboving/code/SCI_SENSORproject/#9bd6e618dff4

--- a/SDFileSystem.lib	Tue Dec 01 15:02:30 2020 +0000
+++ b/SDFileSystem.lib	Thu Dec 03 14:06:20 2020 +0000
@@ -1,1 +1,1 @@
-SDFileSystem#bbe70de9c5dd
+https://os.mbed.com/users/alekboving/code/SDFileSystemproject/#bbe70de9c5dd

--- a/main.cpp	Tue Dec 01 15:02:30 2020 +0000
+++ b/main.cpp	Thu Dec 03 14:06:20 2020 +0000
@@ -75,145 +75,146 @@
     wait(1);
     while(1) {
         // put your main control code here
-
+        if (p_sensor.depth() < 2) {
+            thruster.pulsewidth(1.4/1000);
+            thruster.pulsewidth(1.4/1000);
+            {
+                else (p_sensor.depth() >=2) {
+                    thruster.pulsewidth(1.0/1000);
+                    thruster.pulsewidth(1.0/1000);
+                }
 
 
 
-    }
+            }
 
-}
+        }
 
 //-------------Customized functions---------------------------------------------//----------------------------------------
 ///-----------Welcome menu---------------------///
-void welcome()
-{
-    char buffer[100]= {0};
-    int flag=1;
-    //Flush the port
-    while(BLE.readable()) {
-        BLE.getc();
-    }
-    while(flag) {
-        BLE.printf("### I am alive\r\n");
-        BLE.printf("### Please enter the log file name you want\r\n");
-        if(BLE.readable()) {
-            BLE.scanf("%s",buffer);
-            sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name
+        void welcome() {
+            char buffer[100]= {0};
+            int flag=1;
+            //Flush the port
+            while(BLE.readable()) {
+                BLE.getc();
+            }
+            while(flag) {
+                BLE.printf("### I am alive\r\n");
+                BLE.printf("### Please enter the log file name you want\r\n");
+                if(BLE.readable()) {
+                    BLE.scanf("%s",buffer);
+                    sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name
 
-            flag = 0; //set the flag to 0 to break the while
+                    flag = 0; //set the flag to 0 to break the while
+                }
+                myled=!myled;
+                wait(1);
+            }
+            //print name
+            BLE.printf("### name received\r\n");
+            BLE.printf("### file name and directory is: \r\n %s\r\n",fname); //file name and location
+            //open file test
+            mkdir("/sd/mydir",0777); //keep 0777, this is magic #
+            fp = fopen(fname, "a");
+            if(fp == NULL) {
+                BLE.printf("Could not open file for write\n");
+            } else {
+                BLE.printf("##file open good \n"); //open file and tell if open
+                fprintf(fp, "Hello\r\n");
+                fclose(fp);
+            }
+
+            BLE.printf("### The main program will start in 10 seconds\r\n");
+            wait(5);
         }
-        myled=!myled;
-        wait(1);
-    }
-    //print name
-    BLE.printf("### name received\r\n");
-    BLE.printf("### file name and directory is: \r\n %s\r\n",fname); //file name and location
-    //open file test
-    mkdir("/sd/mydir",0777); //keep 0777, this is magic #
-    fp = fopen(fname, "a");
-    if(fp == NULL) {
-        BLE.printf("Could not open file for write\n");
-    } else {
-        BLE.printf("##file open good \n"); //open file and tell if open
-        fprintf(fp, "Hello\r\n");
-        fclose(fp);
-    }
-
-    BLE.printf("### The main program will start in 10 seconds\r\n");
-    wait(5);
-}
 
 ///-----------log functions---------------------///
-void log_data()
-{
-    //log system time t.read()
-    // log imu data, log sciene data
-    // log pulse width
-    // log pressure sensor data.
-    //science sensor: temp.temp(), light.light()
-    //IMU sensor
+        void log_data() {
+            //log system time t.read()
+            // log imu data, log sciene data
+            // log pulse width
+            // log pressure sensor data.
+            //science sensor: temp.temp(), light.light()
+            //IMU sensor
 
-}
+        }
 
 ///-----------IMU related functions---------------------///
-void IMU_update()
-{
-    IMU.readMag();
-    IMU.readGyro();
-    IMU.readAccel();
-    accel[0] = IMU.calcAccel(IMU.ax);
-    accel[1] = IMU.calcAccel(IMU.ay);
-    accel[2] = -IMU.calcAccel(IMU.az);
-    gyro[0]  = IMU.calcGyro(IMU.gx);
-    gyro[1]  = IMU.calcGyro(IMU.gy);
-    gyro[2]  = -IMU.calcGyro(IMU.gz);
-    mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag);  //mag correction
-    mag[2]   = - mag[2];
-    pose_estimate(euler, accel, gyro, mag);  //pose update
-}
+        void IMU_update() {
+            IMU.readMag();
+            IMU.readGyro();
+            IMU.readAccel();
+            accel[0] = IMU.calcAccel(IMU.ax);
+            accel[1] = IMU.calcAccel(IMU.ay);
+            accel[2] = -IMU.calcAccel(IMU.az);
+            gyro[0]  = IMU.calcGyro(IMU.gx);
+            gyro[1]  = IMU.calcGyro(IMU.gy);
+            gyro[2]  = -IMU.calcGyro(IMU.gz);
+            mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag);  //mag correction
+            mag[2]   = - mag[2];
+            pose_estimate(euler, accel, gyro, mag);  //pose update
+        }
 
-void mag_correction(float mx, float my, float mz, float mag_c[3])
-{
-    float bias[3] = {0.0793,0.0357,0.2333};
-    float scale[3][3] = {{1.0070, 0.0705, 0.0368},
-        {0.0705, 1.0807, 0.0265},
-        {0.0368, 0.0265, 0.9250}
-    };
-    //mag_c = (mag-bias)*scale
+        void mag_correction(float mx, float my, float mz, float mag_c[3]) {
+            float bias[3] = {0.0793,0.0357,0.2333};
+            float scale[3][3] = {{1.0070, 0.0705, 0.0368},
+                {0.0705, 1.0807, 0.0265},
+                {0.0368, 0.0265, 0.9250}
+            };
+            //mag_c = (mag-bias)*scale
 
-    mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
-    mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
-    mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
-}
+            mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
+            mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
+            mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
+        }
 
-void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3])   //pose estimation function
-{
-    euler[0] =  atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
-    euler[1] = - atan2( -accel[0],( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
-    float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
-    float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
-               + (mag[2] * cos(euler[0]) * sin(euler[1]));
-    euler[2] = atan2(Yh, Xh);
-    //convert into degrees
-    euler[0] *= 180.0f / PI;
-    euler[1] *= 180.0f / PI;
-    euler[2] *= 180.0f / PI;
-    //wrap the values to be within 0 to 360.
-    for (int i=0; i<3; i++) {
-        if(euler[i]<=0) {
-            euler[i]=euler[i]+360;
+        void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]) { //pose estimation function
+            euler[0] =  atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
+            euler[1] = - atan2( -accel[0],( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
+            float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
+            float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
+                       + (mag[2] * cos(euler[0]) * sin(euler[1]));
+            euler[2] = atan2(Yh, Xh);
+            //convert into degrees
+            euler[0] *= 180.0f / PI;
+            euler[1] *= 180.0f / PI;
+            euler[2] *= 180.0f / PI;
+            //wrap the values to be within 0 to 360.
+            for (int i=0; i<3; i++) {
+                if(euler[i]<=0) {
+                    euler[i]=euler[i]+360;
+                }
+                if(euler[i]>360) {
+                    euler[i]=euler[i]-360;
+                }
+            }
+
         }
-        if(euler[i]>360) {
-            euler[i]=euler[i]-360;
-        }
-    }
-
-}
 
 ///-----------Control related functions---------------------///
 ////Thruster on control, pw->pulse width in milli-second//
 ////                        pw range between 1 to 1.5//
 ////                       on_time-> thruster on time.
-void thrust_on(float pw, float on_time)   //input is pulse width
-{
-    float pw_max=2.0;
-    if(pw>pw_max) {
-        pw=pw_max; //hard limitation
-    }
-    Timer tt;
-    tt.reset();
-    tt.start();
-    // lets set the pulse width
-    //thruster.period(20.0/1000.00);      // 20 ms period
-    thruster.pulsewidth(pw/1000.00);
-    thruster2.pulsewidth(pw/1000.00);
-    //PWM will be kept until time out
-    while(tt.read()<=on_time) {
-    }
-    //stop the timer
-    tt.stop();
-    //turn off the thruster
-    thruster.pulsewidth(1.0/1000.00);
-    thruster2.pulsewidth(1.0/1000.00);
+        void thrust_on(float pw, float on_time) { //input is pulse width
+            float pw_max=2.0;
+            if(pw>pw_max) {
+                pw=pw_max; //hard limitation
+            }
+            Timer tt;
+            tt.reset();
+            tt.start();
+            // lets set the pulse width
+            //thruster.period(20.0/1000.00);      // 20 ms period
+            thruster.pulsewidth(pw/1000.00);
+            thruster2.pulsewidth(pw/1000.00);
+            //PWM will be kept until time out
+            while(tt.read()<=on_time) {
+            }
+            //stop the timer
+            tt.stop();
+            //turn off the thruster
+            thruster.pulsewidth(1.0/1000.00);
+            thruster2.pulsewidth(1.0/1000.00);
 
-}
+        }