![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ECE 4180 ... Final Design Project
Dependencies: mbed Hovercraft X_NUCLEO_53L0A1 HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- spanda38
- Date:
- 2021-04-23
- Revision:
- 0:8c52f4cf12bd
File content as of revision 0:8c52f4cf12bd:
#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); } } }