Diff: main.cpp

Revision:

0:8c52f4cf12bd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 23 22:47:14 2021 +0000
@@ -0,0 +1,387 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "ultrasonic.h"
+#include "XNucleo53L0A1.h"
+#include "math.h"
+
+//Variable Declaration Centre
+Servo propeller(p21);
+Servo inflator(p22);
+Servo rudder1(p23); //Left Rudder
+Servo rudder2(p24); //Right Rudder
+void goAutonomous(); //Function Prototype
+void command(); // Another Function Prototype
+float maxPropellerVal = 0.2;
+
+Serial blue(p13, p14);
+DigitalOut shdn(p26);
+//I2C sensor pins
+#define VL53L0_I2C_SDA   p28
+#define VL53L0_I2C_SCL   p27
+
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+int deploy = 0; // Disable the System
+int flag = 0;
+
+//Low Level Actuation Centre
+void speedUp()
+{
+    if (deploy) {
+     if (propeller >= maxPropellerVal)
+      { 
+       propeller = maxPropellerVal; 
+      }  else {
+          propeller = propeller + 0.01;
+      }
+    } else {
+        propeller = 0;
+    }
+}
+
+void speedDown()
+{
+    if (deploy) {
+      if (propeller == 0) 
+      { 
+       propeller = 0; 
+      } else { 
+       propeller = propeller - 0.01;
+      }
+    } else {
+        propeller = 0;
+     }
+}
+
+void ventFull()
+{
+    for (float i = 0; i < 0.17; i += 0.01)
+    {
+     inflator = i;
+     deploy = 1;
+     wait(0.5);
+    }
+}
+
+void ventCutOff()
+{
+    inflator = 0;
+    propeller = 0;
+    rudder1 = 0.5;
+    rudder2 = 0.5;
+    deploy = 0;
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+}
+
+void rightTurn()
+{
+    rudder1 = rudder2;
+    if (rudder1 == 1)
+     {
+         rudder1 = 1;
+         rudder2 = 1;
+      } else {
+       rudder1 = rudder1 + 0.05;
+       rudder2 = rudder2 + 0.05;
+    }
+}
+
+void leftTurn()
+{
+    rudder1 = rudder2;
+    if (rudder1 == 0)
+     {
+         rudder1 = 0;
+         rudder2 = 0;
+      } else {
+       rudder1 = rudder1 - 0.05;
+       rudder2 = rudder2 - 0.05;
+    }
+}
+
+void ledFlashers()
+{
+    led1 = !led1;
+    wait(0.2);
+    led2 = !led2;
+    wait(0.1);
+    led3 = !led3;
+    wait(0.1);
+    led4 = !led4;
+    wait(0.2);
+}
+
+//Contingency Trigger...
+
+static XNucleo53L0A1 *board=NULL;
+DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
+int status;
+uint32_t dist;
+int old_dist = 0;
+int warning = 0;
+
+void distance()
+{
+    status = board->sensor_centre->get_distance(&dist);
+     // If less than 10 cm cut off power sound the alarm flash the LEDS!
+     if (dist < 100 && dist != 0 && old_dist < 100 && old_dist != 0) { //No eroneous Warnings
+         warning = 1;
+     } else {
+         warning = 0;
+         led1 = 0;
+         led2 = 0;
+         led3 = 0;
+         led4 = 0;
+     }
+     if (warning == 1)
+    {
+        
+        propeller = 1;
+        inflator = 1;
+        flag = 0; // Cut out the Power Plant...
+        ventCutOff();
+        ledFlashers();
+    }
+    old_dist = dist;
+}
+
+
+    
+
+
+//Boss Loop
+int main()
+{
+    propeller.calibrate();
+    inflator.calibrate();
+    rudder1.calibrate();
+    rudder2.calibrate();
+    
+    rudder1 = 0.5;
+    rudder2 = 0.5;
+    
+    //Sensor Set Up
+    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
+    shdn = 0; 
+    wait(0.1);
+    shdn = 1;
+    wait(0.1);
+    status = board->init_board();
+    while (status) {
+        status = board->init_board();
+    }
+    
+    //End Sensor Set up
+    
+    while(1)
+    {
+        distance(); //Check this as much as you want :-)
+        if (!flag)
+        {
+            flag = 1;
+            propeller = 1;
+            inflator = 1;
+            wait(0.5);
+            propeller = 0;
+            inflator = 0;
+        }
+        if (blue.readable() && blue.getc() == '!')
+        {
+            if (blue.getc() == 'B') {
+                char bnum = blue.getc();
+                if (blue.getc() == '1') 
+                {
+                 switch(bnum)
+                 {
+                     case '5' :  speedUp(); break;
+                     case '6' :  speedDown(); break;
+                     case '1' :  ventFull(); break; //Cushion Deployed
+                     case '2' :  ventCutOff();  break; //Cushion Cutoff
+                     case '3' :  ventCutOff(); goAutonomous(); break;
+                     case '7' :  rightTurn(); break;
+                     case '8' :  leftTurn(); break;
+                 }
+                 blue.getc();
+               }
+            }
+                   
+        }
+
+    }
+    return 0;
+}
+
+
+
+
+float x[4] = {0,0,0,0}; //State Variables.
+float u[2][2] = {{0,0}, {0,0}}; //Inputs that translate to rudder and propeller speeds...
+float y[4][2] = {{0,0},{0,0},{0,0},{0,0}};  // Log this
+int i = 0;
+int j = 0;
+float I = 1; //You can change this 
+//You can modify this as part of a Design Review
+//Force parameters
+float param1 = 7600; // 7600 RPM/Volt
+float param2 = 8659.0135; // Aerodynamic Force
+float param3 = 3.3; //Voltage
+
+//Torque Parameters
+float param4 = 0.0044;
+float param5 = 90; //Range of the servo ... 
+
+void command(float torque, float force)
+{
+    //The actual place where the force and torque and converted to forms the low level controllers can understand
+    float prop_control = force / (param1 * param2 * param3);                   //(propeller * 3.3) * 7600 * 0.00086590135
+    float rudder_control = torque / (param4 * param5);                                  //(rudder1.position() * 0.0644); //Torque Value 
+    printf("%f \n", prop_control);
+    
+    //Do a saturation check...
+    if (rudder_control > 1)
+    {
+        rudder1 = rudder2 = 1;
+        wait(0.2);
+    } else {
+        rudder1 = rudder2 = fabs(rudder_control);
+        wait(0.6);
+    }
+    
+     wait(0.2);
+    
+    if (prop_control > maxPropellerVal)
+    {
+        propeller = 0;
+        propeller = maxPropellerVal - 0.04;
+    }
+    else if (prop_control <= 0) {
+
+        propeller = 0.14;
+    } else {
+        propeller = prop_control;
+    }
+
+} // Done! Enforce the Safety Envelope!
+        
+
+void goAutonomous() 
+{
+    //Sensor Set Up
+    while(1)
+    {
+        distance();
+        if (blue.readable() && blue.getc() == '!')
+        {
+            if (blue.getc() == 'B') {
+                char bnum = blue.getc();
+                if (blue.getc() == '1' && bnum == '4') 
+                {
+                    break; //Go back if 4 is pressed!
+                }
+            }
+        }
+        
+        //Implement the controls aspect here...
+        // Get me the output variables...
+        //dist is y 
+        float T = 0.1;
+        float F = (propeller * param3) * param1 * param2; //This is the force
+        float tau = (rudder1.read() * param4 * param5); //Torque Value
+        float alpha = dist * T + 0.5 * (tau / I) * T * T; //Thats y and alpha ...
+ 
+        int k = (i + 1) % 4;
+        y[k][0] = y[i][0];
+        y[k][1] = y[i][1];
+        y[i][0] =  dist;
+        y[i][1] = alpha; //Log in the data
+        i = k;
+        
+        int w = (j + 1) % 2;
+        u[w][0] = u[j][0];
+        u[w][1] = u[j][1];
+        u[j][0] =  dist;
+        u[j][1] = alpha; //Log in the data
+        j = w;
+
+        
+        //Hybrid Control Algorithms ...
+        
+        if(warning == 1)
+        {
+            ventCutOff();
+        } //Dont move if there is a contingency
+        
+        if (warning == 0) {
+            //There is no obstacle
+            //Plow ahead at optimal speed
+            //propeller = 0.15; //Fill up the cushions
+            if (inflator == 0) {
+              ventFull();
+            }
+            rudder1 = 0.5; //Keep Rudder Positions Straight
+            deploy = 1;
+            //Move now
+            if (propeller != 0.15) {
+               if (propeller == 0) {
+               propeller = 1;
+               propeller = 0;
+               }
+               wait(0.5);
+               propeller = 0.15;
+            } 
+        } if (dist > 100 && dist < 300 && old_dist > 100 && old_dist < 300) {
+            // Now you are in a situation where there is something ahead but not a danger to stop
+            // Rover should make some predictive driving and try to turn
+            float ref_y = dist + 50;
+            float ref_alpha = alpha; //You can change these!
+            
+            float ref_F;
+            float ref_tau; 
+            //Need to compute these and set these as the required angles and rotation RPMs to the controllers...
+            // You can change the parameters on top if needed! No problems with that...
+            // Get the x from there
+            
+            x[0] = -8.5899 * ref_F + 8.5899 * y[2][0] - 8.5899 * y[3][0];
+            x[1] = 0.0307 * ref_F - 0.0234 * y[1][0] - 0.0140 * y[2][0];
+            x[2] = -8.5899 * ref_tau + 8.5899 * y[2][1] - 8.5899 * y[3][1];
+            x[3] = 0.0307 * ref_tau - 0.0234 * y[1][1] - 0.0140 * y[2][1]; // Least Sq Approx Solution BEST LINEAR UNBIASED ESTIMATOR
+   /*      
+   -8.5899         0         0         0    8.5899         0   -8.5899         0
+    0.0307         0   -0.0234         0   -0.0140         0         0         0   = Observability Matrix Least Sq Approx Matrix
+         0   -8.5899         0         0         0    8.5899         0   -8.5899
+         0    0.0307         0   -0.0234         0   -0.0140         0         0
+
+    2.8091    0.0127         0         0
+         0         0    2.8091    0.0127
+   -9.1215    0.0602         0         0
+         0         0   -9.1215    0.0602     = Controllability Matrix Least Norm Matrix
+   -0.1843    0.0065         0         0
+         0         0   -0.1843    0.0065
+    6.4967   -0.0132         0         0
+         0         0    6.4967   -0.0132
+*/
+             ref_tau = 0.4967 * x[2] - 0.0132 * x[3];
+             ref_F = 6.4967 * x[0] - 0.0132 * x[1];
+             command(ref_tau, ref_F); //Computes the values and compensates...
+             
+             ref_tau = 0 * x[0] + 0 * x[1] + (-0.01843) * x[2] + 0.0065 * x[3];
+             ref_F =-0.1843 * x[0] + 0.0065 * x[1] + 0 * x[2] + 0 * x[3];
+             command(ref_tau, ref_F);
+             
+             ref_F = -9.1215 * x[0] + 0.0602  * x[1] + 0 * x[2] + 0 * x[3];
+             ref_tau = 0 * x[0] + 0 * x[1] + (-0.1215) * x[2] + 0.0602 * x[3];
+             command(ref_tau, ref_F);
+             
+             ref_tau = 0 * x[0] + 0 * x[1] + 0.8091 * x[2] + 0.0127 * x[3];
+             ref_F = 2.8091 * x[0] + 0.0127 * x[1] + 0 * x[2] + 0 * x[3];
+             command(ref_tau, ref_F);
+        }       
+    }
+}