main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "ultrasonic.h"
00004 #include "XNucleo53L0A1.h"
00005 #include "math.h"
00006 
00007 //Variable Declaration Centre
00008 Servo propeller(p21);
00009 Servo inflator(p22);
00010 Servo rudder1(p23); //Left Rudder
00011 Servo rudder2(p24); //Right Rudder
00012 void goAutonomous(); //Function Prototype
00013 void command(); // Another Function Prototype
00014 float maxPropellerVal = 0.2;
00015 
00016 Serial blue(p13, p14);
00017 DigitalOut shdn(p26);
00018 //I2C sensor pins
00019 #define VL53L0_I2C_SDA   p28
00020 #define VL53L0_I2C_SCL   p27
00021 
00022 
00023 DigitalOut led1(LED1);
00024 DigitalOut led2(LED2);
00025 DigitalOut led3(LED3);
00026 DigitalOut led4(LED4);
00027 
00028 int deploy = 0; // Disable the System
00029 int flag = 0;
00030 
00031 //Low Level Actuation Centre
00032 void speedUp()
00033 {
00034     if (deploy) {
00035      if (propeller >= maxPropellerVal)
00036       { 
00037        propeller = maxPropellerVal; 
00038       }  else {
00039           propeller = propeller + 0.01;
00040       }
00041     } else {
00042         propeller = 0;
00043     }
00044 }
00045 
00046 void speedDown()
00047 {
00048     if (deploy) {
00049       if (propeller == 0) 
00050       { 
00051        propeller = 0; 
00052       } else { 
00053        propeller = propeller - 0.01;
00054       }
00055     } else {
00056         propeller = 0;
00057      }
00058 }
00059 
00060 void ventFull()
00061 {
00062     for (float i = 0; i < 0.17; i += 0.01)
00063     {
00064      inflator = i;
00065      deploy = 1;
00066      wait(0.5);
00067     }
00068 }
00069 
00070 void ventCutOff()
00071 {
00072     inflator = 0;
00073     propeller = 0;
00074     rudder1 = 0.5;
00075     rudder2 = 0.5;
00076     deploy = 0;
00077     led1 = 0;
00078     led2 = 0;
00079     led3 = 0;
00080     led4 = 0;
00081 }
00082 
00083 void rightTurn()
00084 {
00085     rudder1 = rudder2;
00086     if (rudder1 == 1)
00087      {
00088          rudder1 = 1;
00089          rudder2 = 1;
00090       } else {
00091        rudder1 = rudder1 + 0.05;
00092        rudder2 = rudder2 + 0.05;
00093     }
00094 }
00095 
00096 void leftTurn()
00097 {
00098     rudder1 = rudder2;
00099     if (rudder1 == 0)
00100      {
00101          rudder1 = 0;
00102          rudder2 = 0;
00103       } else {
00104        rudder1 = rudder1 - 0.05;
00105        rudder2 = rudder2 - 0.05;
00106     }
00107 }
00108 
00109 void ledFlashers()
00110 {
00111     led1 = !led1;
00112     wait(0.2);
00113     led2 = !led2;
00114     wait(0.1);
00115     led3 = !led3;
00116     wait(0.1);
00117     led4 = !led4;
00118     wait(0.2);
00119 }
00120 
00121 //Contingency Trigger...
00122 
00123 static XNucleo53L0A1 *board=NULL;
00124 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00125 int status;
00126 uint32_t dist;
00127 int old_dist = 0;
00128 int warning = 0;
00129 
00130 void distance()
00131 {
00132     status = board->sensor_centre->get_distance(&dist);
00133      // If less than 10 cm cut off power sound the alarm flash the LEDS!
00134      if (dist < 100 && dist != 0 && old_dist < 100 && old_dist != 0) { //No eroneous Warnings
00135          warning = 1;
00136      } else {
00137          warning = 0;
00138          led1 = 0;
00139          led2 = 0;
00140          led3 = 0;
00141          led4 = 0;
00142      }
00143      if (warning == 1)
00144     {
00145         
00146         propeller = 1;
00147         inflator = 1;
00148         flag = 0; // Cut out the Power Plant...
00149         ventCutOff();
00150         ledFlashers();
00151     }
00152     old_dist = dist;
00153 }
00154 
00155 
00156     
00157 
00158 
00159 //Boss Loop
00160 int main()
00161 {
00162     propeller.calibrate();
00163     inflator.calibrate();
00164     rudder1.calibrate();
00165     rudder2.calibrate();
00166     
00167     rudder1 = 0.5;
00168     rudder2 = 0.5;
00169     
00170     //Sensor Set Up
00171     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
00172     shdn = 0; 
00173     wait(0.1);
00174     shdn = 1;
00175     wait(0.1);
00176     status = board->init_board();
00177     while (status) {
00178         status = board->init_board();
00179     }
00180     
00181     //End Sensor Set up
00182     
00183     while(1)
00184     {
00185         distance(); //Check this as much as you want :-)
00186         if (!flag)
00187         {
00188             flag = 1;
00189             propeller = 1;
00190             inflator = 1;
00191             wait(0.5);
00192             propeller = 0;
00193             inflator = 0;
00194         }
00195         if (blue.readable() && blue.getc() == '!')
00196         {
00197             if (blue.getc() == 'B') {
00198                 char bnum = blue.getc();
00199                 if (blue.getc() == '1') 
00200                 {
00201                  switch(bnum)
00202                  {
00203                      case '5' :  speedUp(); break;
00204                      case '6' :  speedDown(); break;
00205                      case '1' :  ventFull(); break; //Cushion Deployed
00206                      case '2' :  ventCutOff();  break; //Cushion Cutoff
00207                      case '3' :  ventCutOff(); goAutonomous(); break;
00208                      case '7' :  rightTurn(); break;
00209                      case '8' :  leftTurn(); break;
00210                  }
00211                  blue.getc();
00212                }
00213             }
00214                    
00215         }
00216 
00217     }
00218     return 0;
00219 }
00220 
00221 
00222 
00223 
00224 float x[4] = {0,0,0,0}; //State Variables.
00225 float u[2][2] = {{0,0}, {0,0}}; //Inputs that translate to rudder and propeller speeds...
00226 float y[4][2] = {{0,0},{0,0},{0,0},{0,0}};  // Log this
00227 int i = 0;
00228 int j = 0;
00229 float I = 1; //You can change this 
00230 //You can modify this as part of a Design Review
00231 //Force parameters
00232 float param1 = 7600; // 7600 RPM/Volt
00233 float param2 = 8659.0135; // Aerodynamic Force
00234 float param3 = 3.3; //Voltage
00235 
00236 //Torque Parameters
00237 float param4 = 0.0044;
00238 float param5 = 90; //Range of the servo ... 
00239 
00240 void command(float torque, float force)
00241 {
00242     //The actual place where the force and torque and converted to forms the low level controllers can understand
00243     float prop_control = force / (param1 * param2 * param3);                   //(propeller * 3.3) * 7600 * 0.00086590135
00244     float rudder_control = torque / (param4 * param5);                                  //(rudder1.position() * 0.0644); //Torque Value 
00245     printf("%f \n", prop_control);
00246     
00247     //Do a saturation check...
00248     if (rudder_control > 1)
00249     {
00250         rudder1 = rudder2 = 1;
00251         wait(0.2);
00252     } else {
00253         rudder1 = rudder2 = fabs(rudder_control);
00254         wait(0.6);
00255     }
00256     
00257      wait(0.2);
00258     
00259     if (prop_control > maxPropellerVal)
00260     {
00261         propeller = 0;
00262         propeller = maxPropellerVal - 0.04;
00263     }
00264     else if (prop_control <= 0) {
00265 
00266         propeller = 0.14;
00267     } else {
00268         propeller = prop_control;
00269     }
00270 
00271 } // Done! Enforce the Safety Envelope!
00272         
00273 
00274 void goAutonomous() 
00275 {
00276     //Sensor Set Up
00277     while(1)
00278     {
00279         distance();
00280         if (blue.readable() && blue.getc() == '!')
00281         {
00282             if (blue.getc() == 'B') {
00283                 char bnum = blue.getc();
00284                 if (blue.getc() == '1' && bnum == '4') 
00285                 {
00286                     break; //Go back if 4 is pressed!
00287                 }
00288             }
00289         }
00290         
00291         //Implement the controls aspect here...
00292         // Get me the output variables...
00293         //dist is y 
00294         float T = 0.1;
00295         float F = (propeller * param3) * param1 * param2; //This is the force
00296         float tau = (rudder1.read() * param4 * param5); //Torque Value
00297         float alpha = dist * T + 0.5 * (tau / I) * T * T; //Thats y and alpha ...
00298  
00299         int k = (i + 1) % 4;
00300         y[k][0] = y[i][0];
00301         y[k][1] = y[i][1];
00302         y[i][0] =  dist;
00303         y[i][1] = alpha; //Log in the data
00304         i = k;
00305         
00306         int w = (j + 1) % 2;
00307         u[w][0] = u[j][0];
00308         u[w][1] = u[j][1];
00309         u[j][0] =  dist;
00310         u[j][1] = alpha; //Log in the data
00311         j = w;
00312 
00313         
00314         //Hybrid Control Algorithms ...
00315         
00316         if(warning == 1)
00317         {
00318             ventCutOff();
00319         } //Dont move if there is a contingency
00320         
00321         if (warning == 0) {
00322             //There is no obstacle
00323             //Plow ahead at optimal speed
00324             //propeller = 0.15; //Fill up the cushions
00325             if (inflator == 0) {
00326               ventFull();
00327             }
00328             rudder1 = 0.5; //Keep Rudder Positions Straight
00329             deploy = 1;
00330             //Move now
00331             if (propeller != 0.15) {
00332                if (propeller == 0) {
00333                propeller = 1;
00334                propeller = 0;
00335                }
00336                wait(0.5);
00337                propeller = 0.15;
00338             } 
00339         } if (dist > 100 && dist < 300 && old_dist > 100 && old_dist < 300) {
00340             // Now you are in a situation where there is something ahead but not a danger to stop
00341             // Rover should make some predictive driving and try to turn
00342             float ref_y = dist + 50;
00343             float ref_alpha = alpha; //You can change these!
00344             
00345             float ref_F;
00346             float ref_tau; 
00347             //Need to compute these and set these as the required angles and rotation RPMs to the controllers...
00348             // You can change the parameters on top if needed! No problems with that...
00349             // Get the x from there
00350             
00351             x[0] = -8.5899 * ref_F + 8.5899 * y[2][0] - 8.5899 * y[3][0];
00352             x[1] = 0.0307 * ref_F - 0.0234 * y[1][0] - 0.0140 * y[2][0];
00353             x[2] = -8.5899 * ref_tau + 8.5899 * y[2][1] - 8.5899 * y[3][1];
00354             x[3] = 0.0307 * ref_tau - 0.0234 * y[1][1] - 0.0140 * y[2][1]; // Least Sq Approx Solution BEST LINEAR UNBIASED ESTIMATOR
00355    /*      
00356    -8.5899         0         0         0    8.5899         0   -8.5899         0
00357     0.0307         0   -0.0234         0   -0.0140         0         0         0   = Observability Matrix Least Sq Approx Matrix
00358          0   -8.5899         0         0         0    8.5899         0   -8.5899
00359          0    0.0307         0   -0.0234         0   -0.0140         0         0
00360 
00361     2.8091    0.0127         0         0
00362          0         0    2.8091    0.0127
00363    -9.1215    0.0602         0         0
00364          0         0   -9.1215    0.0602     = Controllability Matrix Least Norm Matrix
00365    -0.1843    0.0065         0         0
00366          0         0   -0.1843    0.0065
00367     6.4967   -0.0132         0         0
00368          0         0    6.4967   -0.0132
00369 */
00370              ref_tau = 0.4967 * x[2] - 0.0132 * x[3];
00371              ref_F = 6.4967 * x[0] - 0.0132 * x[1];
00372              command(ref_tau, ref_F); //Computes the values and compensates...
00373              
00374              ref_tau = 0 * x[0] + 0 * x[1] + (-0.01843) * x[2] + 0.0065 * x[3];
00375              ref_F =-0.1843 * x[0] + 0.0065 * x[1] + 0 * x[2] + 0 * x[3];
00376              command(ref_tau, ref_F);
00377              
00378              ref_F = -9.1215 * x[0] + 0.0602  * x[1] + 0 * x[2] + 0 * x[3];
00379              ref_tau = 0 * x[0] + 0 * x[1] + (-0.1215) * x[2] + 0.0602 * x[3];
00380              command(ref_tau, ref_F);
00381              
00382              ref_tau = 0 * x[0] + 0 * x[1] + 0.8091 * x[2] + 0.0127 * x[3];
00383              ref_F = 2.8091 * x[0] + 0.0127 * x[1] + 0 * x[2] + 0 * x[3];
00384              command(ref_tau, ref_F);
00385         }       
00386     }
00387 }