Diff: main.cpp

Revision:

18:b5f58a274570

Parent:

17:e72c4ac9a39e

Child:

19:c78d3e8353c4

diff -r e72c4ac9a39e -r b5f58a274570 main.cpp
--- a/main.cpp	Thu Dec 02 15:12:59 2021 +0000
+++ b/main.cpp	Fri Dec 03 20:21:19 2021 +0000
@@ -64,14 +64,14 @@
 int main()
 {
 //-----Initialization realted code-------//
-   //inital set the thruster esc to 1ms duty cycle
+    //inital set the thruster esc to 1ms duty cycle
     thruster.period(0.002);      // 2 ms period
-    thruster.pulsewidth(1.0/1000.000);   
-    
+    thruster.pulsewidth(1.0/1000.000);
+
     //Initialize accelerometer:
     accel.init();
     led1=1;
-    
+
     //initialize pressure sensor
     pc.printf("setting the pressure sensor\r\n");
     p_sensor.MS5837Reset();
@@ -80,7 +80,7 @@
     t.start();
     led2=1;
     welcome();
-    
+
     //-----setup ticker-------//
     //setup ticker to separate log and IMU data update.
     //so we could have all our control code in the while loop
@@ -88,7 +88,7 @@
     accel_ticker.attach(&accel_update,0.1);  //10Hz
     log_ticker.attach(&log_data,0.5);
     wait(1);
-    
+
     //Part One
     /*int count=0;
     while(1)
@@ -102,134 +102,133 @@
         return 0;
         }
     }*/
-    
+
     //Part Two
-    while(1){
+    while(1) {
         float current_depth=p_sensor.depth();
         float pw=percent;
         float on_time=3;
-        
-        if (current_depth-vessel_length<2){
+
+        if (current_depth-vessel_length<2) {
             thrust_on(pw,on_time);
-            }
-        if t.read()>1{
+        }
+        if t.read()>60{ //60 because counting in seconds?
             return 0;
-            } 
         }
-    
-    
-    
+    }
+
+    //Part Three
+    while(1) {
+        error_deep = 2.5;    //deep end of error band 2.5m ?
+        error_shallow = 1.5; //shallow end of error band 1.5m?
 
-}
+        if (current_depth <= error_shallow) { //if float is less than 1.5meters deep
+            thrust_on(pw,on_time);               // turn on thruster at set pw and ontime
+            else if (current_depth >= error_deep) //if float is more than 2.5 meters deep
+                wait(1);                          // do nothing for 1 sec.. float will go up for 1 sec
+                else 
+                    pw = ; //very low thruster percentage
+                    thrust_on(pw,on_time); //run thruster at very low percentage to maintain depth      
+        }
+    //
 
 //-------------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
+                }
+                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 5 seconds\r\n");
+            wait(5);
         }
-        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 5 seconds\r\n");
-    wait(5);
-}
 
 ///-----------log functions---------------------///
-void log_data()
-{
-    float current_time=t.read(); 
-    float current_pressure=p_sensor.MS5837_Pressure();
-    float current_depth=p_sensor.depth();
-    float current_pw=percent;
-    float current_Xaccel=accelData[0];
-    float current_Yaccel=accelData[1];
-    float current_Zaccel=accelData[2];
-    float current_temp=temp.temp();
-    float current_light=light.light();
-    
-    fprintf(fp, "$DATA, %f, %f, %f, %f, %f, %f, %f, %f, %f\r\n", current_time,current_pressure,current_depth,current_Xaccel,current_Yaccel,current_Zaccel,current_temp,current_light,current_pw);
-    
-    // log system time t.read()
-    // log imu data, log science data
-    // log pulse width
-    // log pressure sensor data.
-    // science sensor: temp.temp(), light.light()
-    // IMU sensor
+        void log_data() {
+            float current_time=t.read();
+            float current_pressure=p_sensor.MS5837_Pressure();
+            float current_depth=p_sensor.depth();
+            float current_pw=percent;
+            float current_Xaccel=accelData[0];
+            float current_Yaccel=accelData[1];
+            float current_Zaccel=accelData[2];
+            float current_temp=temp.temp();
+            float current_light=light.light();
 
-}
+            fprintf(fp, "$DATA, %f, %f, %f, %f, %f, %f, %f, %f, %f\r\n", current_time,current_pressure,current_depth,current_Xaccel,current_Yaccel,current_Zaccel,current_temp,current_light,current_pw);
+
+            // log system time t.read()
+            // log imu data, log science data
+            // log pulse width
+            // log pressure sensor data.
+            // science sensor: temp.temp(), light.light()
+            // IMU sensor
+
+        }
 
 ///-----------acceleromter related functions---------------------///
-void accel_update()
-{
-    accelData[0] = accel.readX();
-    accelData[1] = accel.readY();
-    accelData[2] = accel.readZ(); 
-}
+        void accel_update() {
+            accelData[0] = accel.readX();
+            accelData[1] = accel.readY();
+            accelData[2] = accel.readZ();
+        }
 
 ///-----------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)
-    {
-        
-        pc.printf("thruster on?...mack is cool\r\n"); //print check
-    }
-    //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) {
+
+                pc.printf("thruster on?...mack is cool\r\n"); //print check
+            }
+            //stop the timer
+            tt.stop();
+            //turn off the thruster
+            thruster.pulsewidth(1.0/1000.00);
+            thruster2.pulsewidth(1.0/1000.00);
+
+        }
 
 ///void thrust_off(float pw, float on_time)  //input is pulse width
 //
\ No newline at end of file