Siddhanta Panda
ECE 4180 ... Final Design Project
Dependencies: mbed Hovercraft X_NUCLEO_53L0A1 HC_SR04_Ultrasonic_Library
Revision 0:8c52f4cf12bd, committed 2021-04-23
- Comitter:
- spanda38
- Date:
- Fri Apr 23 22:47:14 2021 +0000
- Commit message:
- ECE 4180 Final Project;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Fri Apr 23 22:47:14 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Apr 23 22:47:14 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/spanda38/code/Hovercraft/#8d86483df237
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_53L0A1.lib Fri Apr 23 22:47:14 2021 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/X_NUCLEO_53L0A1/#27d3d95c8593
--- /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);
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Apr 23 22:47:14 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file