main.cpp

00001 //OCE Project Demo
00002 //Kelsey Rose
00003 //Mark Schwarzer
00004 //Jason Schaedler
00005 
00006 #include "mbed.h"
00007 #include "LSM9DS1.h"  //IMU library
00008 #include "MS5837.h"     //pressure sensor library
00009 #include "SCI_SENSOR.h"     //science sensor
00010 #include "SDFileSystem.h"   // SD card
00011 
00012 DigitalOut myled(LED1);
00013 Serial pc(USBTX, USBRX);    //initial serial
00014 Serial BLE(p13,p14);        //Bluetooth
00015 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);  //initial IMU
00016 LM19 t_sensor(p19);
00017 PhotoCell photo(p20);
00018 MS5837 p_sensor(p9, p10, ms5837_addr_no_CS);  //pressure sensor
00019 PwmOut thruster(p21);  //set PWM pin    //max 1.3ms min 1.1ms
00020 PwmOut thruster2(p22); //set PWM pin
00021 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00022 
00023 //global ticker
00024 Ticker log_ticker;
00025 Ticker imu_ticker;
00026 // global timer
00027 Timer t;
00028 //MS5837 p_sensor(p9, p10, ms5837_addr_no_CS);
00029 Timer ttt;
00030 //timer for running thrusters
00031 ///File 
00032 FILE *fp;
00033 char fname[100];
00034 float PI = 3.14159265358979323846f;
00035 float pw;
00036 
00037 //float operation parameters
00038 float target_depth=0;   //global target depth default 0
00039 int yo_num=0;           //global yo_num default 0
00040 float thrust_on_time=0; //global thrust_on time default 0
00041 float accel[3], mag[3], gyro[3], euler[3];  //global IMU data
00042 
00043 //functions
00044 void welcome();
00045 void log_data();
00046 //IMU related
00047 void IMU_update(); //update IMU related varibles. we use imu_ticker to call this function
00048 void mag_correction(float mx, float my, float mz, float mag_c[3]); //raw mag -> mag[3], mag_c[3] calibrated
00049 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]);  //pose estimation function
00050 //Control related functions
00051 void thrust_on(float pw, float on_time);  //input is pulse width
00052 
00053 //-------------Main functions-----------------------------------------------------------------------------------------
00054 int main()
00055 {
00056 //-----Initialization realted code-------//
00057    //inital set the thruster esc to 1ms duty cycle
00058     thruster.period(0.002);      // 2 ms period
00059     thruster.pulsewidth(1.0/1000.000);    /////IMU initial and begin
00060     thruster2.period(0.002);      // 2 ms period
00061     thruster2.pulsewidth(1.0/1000.000);    /////IMU initial and begin
00062     IMU.begin();
00063     IMU.calibrate(true);
00064     myled=1;
00065     //initialize pressure sensor
00066     pc.printf("setting the pressure sensor\r\n");
00067     p_sensor.MS5837Reset();
00068     p_sensor.MS5837Init();
00069     pc.printf("settting the tickers\r\n");
00070     t.start();
00071     myled=0;
00072     welcome();
00073     //-----setup ticker-------//
00074     //setup ticker to separate log and IMU data update.
00075     //so we could have all our control code in the while loop
00076     //   //log at 2 Hz
00077     imu_ticker.attach(&IMU_update,0.1);  //10Hz
00078     log_ticker.attach(&log_data,0.5);
00079     wait(1);
00080     while(1)
00081     { 
00082         ttt.start();
00083         p_sensor.Barometer_MS5837();
00084         //Depth Holding
00085         if (p_sensor.depth()>2.0) {
00086             pw = 1.1;
00087             thruster.pulsewidth(pw/1000.00);
00088             thruster2.pulsewidth(pw/1000.00);
00089         }
00090         else if(p_sensor.depth()<2.0) {
00091             pw = 1.5;
00092             thruster.pulsewidth(pw/1000.00);
00093             thruster2.pulsewidth(pw/1000.00);
00094         }
00095         else if(p_sensor.depth()==2.0) {
00096             pw = 1.3;
00097             thruster.pulsewidth(pw/1000.00);
00098             thruster2.pulsewidth(pw/1000.00);
00099         }
00100     
00101     if (ttt.read()>=180)
00102     {   
00103     
00104             //stop the timer
00105             ttt.stop();
00106             //turn off the thruster
00107             pw = 1.0;
00108             thruster.pulsewidth(pw/1000.00);
00109             thruster2.pulsewidth(pw/1000.00);
00110     }
00111  /*
00112         printf("Pressure: "); 
00113         printf("%f",p_sensor.MS5837_Pressure()); 
00114         printf(" mbar");
00115           
00116         printf("Temperature: "); 
00117         printf("%f",p_sensor.MS5837_Temperature()); 
00118         printf(" deg C ");
00119           
00120         printf("Depth: "); 
00121         printf("%f",p_sensor.depth()); 
00122         printf(" m ");
00123           
00124         printf("Altitude: "); 
00125         printf("%f",p_sensor.altitude()); 
00126         printf(" m above mean sea level\r\n");
00127 */       
00128         myled=!myled;
00129         wait(1);
00130     }
00131     }
00132 
00133     
00134 
00135 //-------------Customized functions---------------------------------------------//----------------------------------------
00136 ///-----------Welcome menu---------------------///
00137 void welcome()
00138 {
00139     char buffer[100]={0};
00140     int flag=1;
00141     //Flush the port
00142     while(BLE.readable())
00143     {
00144     BLE.getc();
00145     }
00146     while(flag)
00147     {
00148         BLE.printf("### I am alive\r\n");
00149         BLE.printf("### Please enter the log file name you want\r\n");
00150         if(BLE.readable())
00151         {
00152             BLE.scanf("%s",buffer);
00153             sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name
00154 
00155             flag = 0; //set the flag to 0 to break the while
00156         }
00157         myled=!myled;
00158         wait(1);
00159     }
00160     //print name
00161     BLE.printf("### name received\r\n");
00162     BLE.printf("### file name and directory is: \r\n %s\r\n",fname); //file name and location
00163     //open file test
00164     mkdir("/sd/mydir",0777); //keep 0777, this is magic #
00165     fp = fopen(fname, "a");
00166     if(fp == NULL){
00167         BLE.printf("Could not open file for write\n");
00168     }
00169     else
00170     {
00171         BLE.printf("##file open good \n"); //open file and tell if open
00172         fprintf(fp, "Hello\r\n");
00173         fclose(fp);
00174     }
00175 
00176     BLE.printf("### The main program will start in 10 seconds\r\n");
00177     wait(5);
00178 }
00179 
00180 ///-----------log functions---------------------///
00181 void log_data()
00182 {
00183     fp= fopen(fname, "a");
00184     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());
00185     fclose(fp);
00186     
00187 }
00188 
00189 ///-----------IMU related functions---------------------///
00190 void IMU_update()
00191 {
00192     IMU.readMag();
00193     IMU.readGyro();
00194     IMU.readAccel();
00195     accel[0] = IMU.calcAccel(IMU.ax);
00196     accel[1] = IMU.calcAccel(IMU.ay);
00197     accel[2] = -IMU.calcAccel(IMU.az);
00198     gyro[0]  = IMU.calcGyro(IMU.gx);
00199     gyro[1]  = IMU.calcGyro(IMU.gy);
00200     gyro[2]  = -IMU.calcGyro(IMU.gz);
00201     mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag);  //mag correction
00202     mag[2]   = - mag[2];
00203     pose_estimate(euler, accel, gyro, mag);  //pose update
00204 }
00205 
00206 void mag_correction(float mx, float my, float mz, float mag_c[3])
00207 {
00208     float bias[3] = {0.0793,0.0357,0.2333};
00209     float scale[3][3] = {{1.0070, 0.0705, 0.0368},
00210                          {0.0705, 1.0807, 0.0265},
00211                          {0.0368, 0.0265, 0.9250}};
00212     //mag_c = (mag-bias)*scale
00213 
00214     mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
00215     mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
00216     mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
00217 }
00218 
00219 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3])  //pose estimation function
00220 {
00221         euler[0] =  atan2 (accel[1] , accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
00222         euler[1] = - atan2( -accel[0] ,( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
00223         float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
00224         float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
00225                     + (mag[2] * cos(euler[0]) * sin(euler[1]));
00226         euler[2] = atan2(Yh, Xh);
00227         //convert into degrees
00228         euler[0] *= 180.0f / PI;
00229         euler[1] *= 180.0f / PI;
00230         euler[2] *= 180.0f / PI;
00231         //wrap the values to be within 0 to 360.
00232         for (int i=0;i<3;i++)
00233         {
00234             if(euler[i]<=0)
00235             {
00236                 euler[i]=euler[i]+360;
00237             }
00238             if(euler[i]>360)
00239             {
00240                 euler[i]=euler[i]-360;
00241             }
00242         }
00243 
00244 }
00245 
00246 ///-----------Control related functions---------------------///
00247 ////Thruster on control, pw->pulse width in milli-second//
00248 ////                        pw range between 1 to 1.5//
00249 ////                       on_time-> thruster on time.
00250 void thrust_on(float on_time)  //input is pulse width
00251 {
00252     float pw_max=2.0;
00253     if(pw>pw_max)
00254     {
00255         pw=pw_max; //hard limitation
00256     }
00257     Timer tt;
00258     tt.reset();
00259     tt.start();
00260     // lets set the pulse width
00261     thruster.period(20.0/1000.00);      // 20 ms period
00262     thruster.pulsewidth(pw/1000.00);
00263     thruster2.pulsewidth(pw/1000.00);
00264     //PWM will be kept until time out
00265     while(tt.read()<=on_time)
00266     {
00267     }
00268     //stop the timer
00269     tt.stop();
00270     //turn off the thruster
00271     thruster.pulsewidth(1.0/1000.00);
00272     thruster2.pulsewidth(1.0/1000.00);
00273 
00274 }