Siddhanta Panda / Mbed 2 deprecated TheHovercraft

ECE 4180 ... Final Design Project

Dependencies:   mbed Hovercraft X_NUCLEO_53L0A1 HC_SR04_Ultrasonic_Library

main.cpp@0:8c52f4cf12bd, 2021-04-23 (annotated)

Committer:
spanda38
Date:
Fri Apr 23 22:47:14 2021 +0000
Revision:
0:8c52f4cf12bd
ECE 4180 Final Project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
spanda38 0:8c52f4cf12bd 1 #include "mbed.h"
spanda38 0:8c52f4cf12bd 2 #include "Servo.h"
spanda38 0:8c52f4cf12bd 3 #include "ultrasonic.h"
spanda38 0:8c52f4cf12bd 4 #include "XNucleo53L0A1.h"
spanda38 0:8c52f4cf12bd 5 #include "math.h"
spanda38 0:8c52f4cf12bd 6
spanda38 0:8c52f4cf12bd 7 //Variable Declaration Centre
spanda38 0:8c52f4cf12bd 8 Servo propeller(p21);
spanda38 0:8c52f4cf12bd 9 Servo inflator(p22);
spanda38 0:8c52f4cf12bd 10 Servo rudder1(p23); //Left Rudder
spanda38 0:8c52f4cf12bd 11 Servo rudder2(p24); //Right Rudder
spanda38 0:8c52f4cf12bd 12 void goAutonomous(); //Function Prototype
spanda38 0:8c52f4cf12bd 13 void command(); // Another Function Prototype
spanda38 0:8c52f4cf12bd 14 float maxPropellerVal = 0.2;
spanda38 0:8c52f4cf12bd 15
spanda38 0:8c52f4cf12bd 16 Serial blue(p13, p14);
spanda38 0:8c52f4cf12bd 17 DigitalOut shdn(p26);
spanda38 0:8c52f4cf12bd 18 //I2C sensor pins
spanda38 0:8c52f4cf12bd 19 #define VL53L0_I2C_SDA p28
spanda38 0:8c52f4cf12bd 20 #define VL53L0_I2C_SCL p27
spanda38 0:8c52f4cf12bd 21
spanda38 0:8c52f4cf12bd 22
spanda38 0:8c52f4cf12bd 23 DigitalOut led1(LED1);
spanda38 0:8c52f4cf12bd 24 DigitalOut led2(LED2);
spanda38 0:8c52f4cf12bd 25 DigitalOut led3(LED3);
spanda38 0:8c52f4cf12bd 26 DigitalOut led4(LED4);
spanda38 0:8c52f4cf12bd 27
spanda38 0:8c52f4cf12bd 28 int deploy = 0; // Disable the System
spanda38 0:8c52f4cf12bd 29 int flag = 0;
spanda38 0:8c52f4cf12bd 30
spanda38 0:8c52f4cf12bd 31 //Low Level Actuation Centre
spanda38 0:8c52f4cf12bd 32 void speedUp()
spanda38 0:8c52f4cf12bd 33 {
spanda38 0:8c52f4cf12bd 34 if (deploy) {
spanda38 0:8c52f4cf12bd 35 if (propeller >= maxPropellerVal)
spanda38 0:8c52f4cf12bd 36 {
spanda38 0:8c52f4cf12bd 37 propeller = maxPropellerVal;
spanda38 0:8c52f4cf12bd 38 } else {
spanda38 0:8c52f4cf12bd 39 propeller = propeller + 0.01;
spanda38 0:8c52f4cf12bd 40 }
spanda38 0:8c52f4cf12bd 41 } else {
spanda38 0:8c52f4cf12bd 42 propeller = 0;
spanda38 0:8c52f4cf12bd 43 }
spanda38 0:8c52f4cf12bd 44 }
spanda38 0:8c52f4cf12bd 45
spanda38 0:8c52f4cf12bd 46 void speedDown()
spanda38 0:8c52f4cf12bd 47 {
spanda38 0:8c52f4cf12bd 48 if (deploy) {
spanda38 0:8c52f4cf12bd 49 if (propeller == 0)
spanda38 0:8c52f4cf12bd 50 {
spanda38 0:8c52f4cf12bd 51 propeller = 0;
spanda38 0:8c52f4cf12bd 52 } else {
spanda38 0:8c52f4cf12bd 53 propeller = propeller - 0.01;
spanda38 0:8c52f4cf12bd 54 }
spanda38 0:8c52f4cf12bd 55 } else {
spanda38 0:8c52f4cf12bd 56 propeller = 0;
spanda38 0:8c52f4cf12bd 57 }
spanda38 0:8c52f4cf12bd 58 }
spanda38 0:8c52f4cf12bd 59
spanda38 0:8c52f4cf12bd 60 void ventFull()
spanda38 0:8c52f4cf12bd 61 {
spanda38 0:8c52f4cf12bd 62 for (float i = 0; i < 0.17; i += 0.01)
spanda38 0:8c52f4cf12bd 63 {
spanda38 0:8c52f4cf12bd 64 inflator = i;
spanda38 0:8c52f4cf12bd 65 deploy = 1;
spanda38 0:8c52f4cf12bd 66 wait(0.5);
spanda38 0:8c52f4cf12bd 67 }
spanda38 0:8c52f4cf12bd 68 }
spanda38 0:8c52f4cf12bd 69
spanda38 0:8c52f4cf12bd 70 void ventCutOff()
spanda38 0:8c52f4cf12bd 71 {
spanda38 0:8c52f4cf12bd 72 inflator = 0;
spanda38 0:8c52f4cf12bd 73 propeller = 0;
spanda38 0:8c52f4cf12bd 74 rudder1 = 0.5;
spanda38 0:8c52f4cf12bd 75 rudder2 = 0.5;
spanda38 0:8c52f4cf12bd 76 deploy = 0;
spanda38 0:8c52f4cf12bd 77 led1 = 0;
spanda38 0:8c52f4cf12bd 78 led2 = 0;
spanda38 0:8c52f4cf12bd 79 led3 = 0;
spanda38 0:8c52f4cf12bd 80 led4 = 0;
spanda38 0:8c52f4cf12bd 81 }
spanda38 0:8c52f4cf12bd 82
spanda38 0:8c52f4cf12bd 83 void rightTurn()
spanda38 0:8c52f4cf12bd 84 {
spanda38 0:8c52f4cf12bd 85 rudder1 = rudder2;
spanda38 0:8c52f4cf12bd 86 if (rudder1 == 1)
spanda38 0:8c52f4cf12bd 87 {
spanda38 0:8c52f4cf12bd 88 rudder1 = 1;
spanda38 0:8c52f4cf12bd 89 rudder2 = 1;
spanda38 0:8c52f4cf12bd 90 } else {
spanda38 0:8c52f4cf12bd 91 rudder1 = rudder1 + 0.05;
spanda38 0:8c52f4cf12bd 92 rudder2 = rudder2 + 0.05;
spanda38 0:8c52f4cf12bd 93 }
spanda38 0:8c52f4cf12bd 94 }
spanda38 0:8c52f4cf12bd 95
spanda38 0:8c52f4cf12bd 96 void leftTurn()
spanda38 0:8c52f4cf12bd 97 {
spanda38 0:8c52f4cf12bd 98 rudder1 = rudder2;
spanda38 0:8c52f4cf12bd 99 if (rudder1 == 0)
spanda38 0:8c52f4cf12bd 100 {
spanda38 0:8c52f4cf12bd 101 rudder1 = 0;
spanda38 0:8c52f4cf12bd 102 rudder2 = 0;
spanda38 0:8c52f4cf12bd 103 } else {
spanda38 0:8c52f4cf12bd 104 rudder1 = rudder1 - 0.05;
spanda38 0:8c52f4cf12bd 105 rudder2 = rudder2 - 0.05;
spanda38 0:8c52f4cf12bd 106 }
spanda38 0:8c52f4cf12bd 107 }
spanda38 0:8c52f4cf12bd 108
spanda38 0:8c52f4cf12bd 109 void ledFlashers()
spanda38 0:8c52f4cf12bd 110 {
spanda38 0:8c52f4cf12bd 111 led1 = !led1;
spanda38 0:8c52f4cf12bd 112 wait(0.2);
spanda38 0:8c52f4cf12bd 113 led2 = !led2;
spanda38 0:8c52f4cf12bd 114 wait(0.1);
spanda38 0:8c52f4cf12bd 115 led3 = !led3;
spanda38 0:8c52f4cf12bd 116 wait(0.1);
spanda38 0:8c52f4cf12bd 117 led4 = !led4;
spanda38 0:8c52f4cf12bd 118 wait(0.2);
spanda38 0:8c52f4cf12bd 119 }
spanda38 0:8c52f4cf12bd 120
spanda38 0:8c52f4cf12bd 121 //Contingency Trigger...
spanda38 0:8c52f4cf12bd 122
spanda38 0:8c52f4cf12bd 123 static XNucleo53L0A1 *board=NULL;
spanda38 0:8c52f4cf12bd 124 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
spanda38 0:8c52f4cf12bd 125 int status;
spanda38 0:8c52f4cf12bd 126 uint32_t dist;
spanda38 0:8c52f4cf12bd 127 int old_dist = 0;
spanda38 0:8c52f4cf12bd 128 int warning = 0;
spanda38 0:8c52f4cf12bd 129
spanda38 0:8c52f4cf12bd 130 void distance()
spanda38 0:8c52f4cf12bd 131 {
spanda38 0:8c52f4cf12bd 132 status = board->sensor_centre->get_distance(&dist);
spanda38 0:8c52f4cf12bd 133 // If less than 10 cm cut off power sound the alarm flash the LEDS!
spanda38 0:8c52f4cf12bd 134 if (dist < 100 && dist != 0 && old_dist < 100 && old_dist != 0) { //No eroneous Warnings
spanda38 0:8c52f4cf12bd 135 warning = 1;
spanda38 0:8c52f4cf12bd 136 } else {
spanda38 0:8c52f4cf12bd 137 warning = 0;
spanda38 0:8c52f4cf12bd 138 led1 = 0;
spanda38 0:8c52f4cf12bd 139 led2 = 0;
spanda38 0:8c52f4cf12bd 140 led3 = 0;
spanda38 0:8c52f4cf12bd 141 led4 = 0;
spanda38 0:8c52f4cf12bd 142 }
spanda38 0:8c52f4cf12bd 143 if (warning == 1)
spanda38 0:8c52f4cf12bd 144 {
spanda38 0:8c52f4cf12bd 145
spanda38 0:8c52f4cf12bd 146 propeller = 1;
spanda38 0:8c52f4cf12bd 147 inflator = 1;
spanda38 0:8c52f4cf12bd 148 flag = 0; // Cut out the Power Plant...
spanda38 0:8c52f4cf12bd 149 ventCutOff();
spanda38 0:8c52f4cf12bd 150 ledFlashers();
spanda38 0:8c52f4cf12bd 151 }
spanda38 0:8c52f4cf12bd 152 old_dist = dist;
spanda38 0:8c52f4cf12bd 153 }
spanda38 0:8c52f4cf12bd 154
spanda38 0:8c52f4cf12bd 155
spanda38 0:8c52f4cf12bd 156
spanda38 0:8c52f4cf12bd 157
spanda38 0:8c52f4cf12bd 158
spanda38 0:8c52f4cf12bd 159 //Boss Loop
spanda38 0:8c52f4cf12bd 160 int main()
spanda38 0:8c52f4cf12bd 161 {
spanda38 0:8c52f4cf12bd 162 propeller.calibrate();
spanda38 0:8c52f4cf12bd 163 inflator.calibrate();
spanda38 0:8c52f4cf12bd 164 rudder1.calibrate();
spanda38 0:8c52f4cf12bd 165 rudder2.calibrate();
spanda38 0:8c52f4cf12bd 166
spanda38 0:8c52f4cf12bd 167 rudder1 = 0.5;
spanda38 0:8c52f4cf12bd 168 rudder2 = 0.5;
spanda38 0:8c52f4cf12bd 169
spanda38 0:8c52f4cf12bd 170 //Sensor Set Up
spanda38 0:8c52f4cf12bd 171 board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
spanda38 0:8c52f4cf12bd 172 shdn = 0;
spanda38 0:8c52f4cf12bd 173 wait(0.1);
spanda38 0:8c52f4cf12bd 174 shdn = 1;
spanda38 0:8c52f4cf12bd 175 wait(0.1);
spanda38 0:8c52f4cf12bd 176 status = board->init_board();
spanda38 0:8c52f4cf12bd 177 while (status) {
spanda38 0:8c52f4cf12bd 178 status = board->init_board();
spanda38 0:8c52f4cf12bd 179 }
spanda38 0:8c52f4cf12bd 180
spanda38 0:8c52f4cf12bd 181 //End Sensor Set up
spanda38 0:8c52f4cf12bd 182
spanda38 0:8c52f4cf12bd 183 while(1)
spanda38 0:8c52f4cf12bd 184 {
spanda38 0:8c52f4cf12bd 185 distance(); //Check this as much as you want :-)
spanda38 0:8c52f4cf12bd 186 if (!flag)
spanda38 0:8c52f4cf12bd 187 {
spanda38 0:8c52f4cf12bd 188 flag = 1;
spanda38 0:8c52f4cf12bd 189 propeller = 1;
spanda38 0:8c52f4cf12bd 190 inflator = 1;
spanda38 0:8c52f4cf12bd 191 wait(0.5);
spanda38 0:8c52f4cf12bd 192 propeller = 0;
spanda38 0:8c52f4cf12bd 193 inflator = 0;
spanda38 0:8c52f4cf12bd 194 }
spanda38 0:8c52f4cf12bd 195 if (blue.readable() && blue.getc() == '!')
spanda38 0:8c52f4cf12bd 196 {
spanda38 0:8c52f4cf12bd 197 if (blue.getc() == 'B') {
spanda38 0:8c52f4cf12bd 198 char bnum = blue.getc();
spanda38 0:8c52f4cf12bd 199 if (blue.getc() == '1')
spanda38 0:8c52f4cf12bd 200 {
spanda38 0:8c52f4cf12bd 201 switch(bnum)
spanda38 0:8c52f4cf12bd 202 {
spanda38 0:8c52f4cf12bd 203 case '5' : speedUp(); break;
spanda38 0:8c52f4cf12bd 204 case '6' : speedDown(); break;
spanda38 0:8c52f4cf12bd 205 case '1' : ventFull(); break; //Cushion Deployed
spanda38 0:8c52f4cf12bd 206 case '2' : ventCutOff(); break; //Cushion Cutoff
spanda38 0:8c52f4cf12bd 207 case '3' : ventCutOff(); goAutonomous(); break;
spanda38 0:8c52f4cf12bd 208 case '7' : rightTurn(); break;
spanda38 0:8c52f4cf12bd 209 case '8' : leftTurn(); break;
spanda38 0:8c52f4cf12bd 210 }
spanda38 0:8c52f4cf12bd 211 blue.getc();
spanda38 0:8c52f4cf12bd 212 }
spanda38 0:8c52f4cf12bd 213 }
spanda38 0:8c52f4cf12bd 214
spanda38 0:8c52f4cf12bd 215 }
spanda38 0:8c52f4cf12bd 216
spanda38 0:8c52f4cf12bd 217 }
spanda38 0:8c52f4cf12bd 218 return 0;
spanda38 0:8c52f4cf12bd 219 }
spanda38 0:8c52f4cf12bd 220
spanda38 0:8c52f4cf12bd 221
spanda38 0:8c52f4cf12bd 222
spanda38 0:8c52f4cf12bd 223
spanda38 0:8c52f4cf12bd 224 float x[4] = {0,0,0,0}; //State Variables.
spanda38 0:8c52f4cf12bd 225 float u[2][2] = {{0,0}, {0,0}}; //Inputs that translate to rudder and propeller speeds...
spanda38 0:8c52f4cf12bd 226 float y[4][2] = {{0,0},{0,0},{0,0},{0,0}}; // Log this
spanda38 0:8c52f4cf12bd 227 int i = 0;
spanda38 0:8c52f4cf12bd 228 int j = 0;
spanda38 0:8c52f4cf12bd 229 float I = 1; //You can change this
spanda38 0:8c52f4cf12bd 230 //You can modify this as part of a Design Review
spanda38 0:8c52f4cf12bd 231 //Force parameters
spanda38 0:8c52f4cf12bd 232 float param1 = 7600; // 7600 RPM/Volt
spanda38 0:8c52f4cf12bd 233 float param2 = 8659.0135; // Aerodynamic Force
spanda38 0:8c52f4cf12bd 234 float param3 = 3.3; //Voltage
spanda38 0:8c52f4cf12bd 235
spanda38 0:8c52f4cf12bd 236 //Torque Parameters
spanda38 0:8c52f4cf12bd 237 float param4 = 0.0044;
spanda38 0:8c52f4cf12bd 238 float param5 = 90; //Range of the servo ...
spanda38 0:8c52f4cf12bd 239
spanda38 0:8c52f4cf12bd 240 void command(float torque, float force)
spanda38 0:8c52f4cf12bd 241 {
spanda38 0:8c52f4cf12bd 242 //The actual place where the force and torque and converted to forms the low level controllers can understand
spanda38 0:8c52f4cf12bd 243 float prop_control = force / (param1 * param2 * param3); //(propeller * 3.3) * 7600 * 0.00086590135
spanda38 0:8c52f4cf12bd 244 float rudder_control = torque / (param4 * param5); //(rudder1.position() * 0.0644); //Torque Value
spanda38 0:8c52f4cf12bd 245 printf("%f \n", prop_control);
spanda38 0:8c52f4cf12bd 246
spanda38 0:8c52f4cf12bd 247 //Do a saturation check...
spanda38 0:8c52f4cf12bd 248 if (rudder_control > 1)
spanda38 0:8c52f4cf12bd 249 {
spanda38 0:8c52f4cf12bd 250 rudder1 = rudder2 = 1;
spanda38 0:8c52f4cf12bd 251 wait(0.2);
spanda38 0:8c52f4cf12bd 252 } else {
spanda38 0:8c52f4cf12bd 253 rudder1 = rudder2 = fabs(rudder_control);
spanda38 0:8c52f4cf12bd 254 wait(0.6);
spanda38 0:8c52f4cf12bd 255 }
spanda38 0:8c52f4cf12bd 256
spanda38 0:8c52f4cf12bd 257 wait(0.2);
spanda38 0:8c52f4cf12bd 258
spanda38 0:8c52f4cf12bd 259 if (prop_control > maxPropellerVal)
spanda38 0:8c52f4cf12bd 260 {
spanda38 0:8c52f4cf12bd 261 propeller = 0;
spanda38 0:8c52f4cf12bd 262 propeller = maxPropellerVal - 0.04;
spanda38 0:8c52f4cf12bd 263 }
spanda38 0:8c52f4cf12bd 264 else if (prop_control <= 0) {
spanda38 0:8c52f4cf12bd 265
spanda38 0:8c52f4cf12bd 266 propeller = 0.14;
spanda38 0:8c52f4cf12bd 267 } else {
spanda38 0:8c52f4cf12bd 268 propeller = prop_control;
spanda38 0:8c52f4cf12bd 269 }
spanda38 0:8c52f4cf12bd 270
spanda38 0:8c52f4cf12bd 271 } // Done! Enforce the Safety Envelope!
spanda38 0:8c52f4cf12bd 272
spanda38 0:8c52f4cf12bd 273
spanda38 0:8c52f4cf12bd 274 void goAutonomous()
spanda38 0:8c52f4cf12bd 275 {
spanda38 0:8c52f4cf12bd 276 //Sensor Set Up
spanda38 0:8c52f4cf12bd 277 while(1)
spanda38 0:8c52f4cf12bd 278 {
spanda38 0:8c52f4cf12bd 279 distance();
spanda38 0:8c52f4cf12bd 280 if (blue.readable() && blue.getc() == '!')
spanda38 0:8c52f4cf12bd 281 {
spanda38 0:8c52f4cf12bd 282 if (blue.getc() == 'B') {
spanda38 0:8c52f4cf12bd 283 char bnum = blue.getc();
spanda38 0:8c52f4cf12bd 284 if (blue.getc() == '1' && bnum == '4')
spanda38 0:8c52f4cf12bd 285 {
spanda38 0:8c52f4cf12bd 286 break; //Go back if 4 is pressed!
spanda38 0:8c52f4cf12bd 287 }
spanda38 0:8c52f4cf12bd 288 }
spanda38 0:8c52f4cf12bd 289 }
spanda38 0:8c52f4cf12bd 290
spanda38 0:8c52f4cf12bd 291 //Implement the controls aspect here...
spanda38 0:8c52f4cf12bd 292 // Get me the output variables...
spanda38 0:8c52f4cf12bd 293 //dist is y
spanda38 0:8c52f4cf12bd 294 float T = 0.1;
spanda38 0:8c52f4cf12bd 295 float F = (propeller * param3) * param1 * param2; //This is the force
spanda38 0:8c52f4cf12bd 296 float tau = (rudder1.read() * param4 * param5); //Torque Value
spanda38 0:8c52f4cf12bd 297 float alpha = dist * T + 0.5 * (tau / I) * T * T; //Thats y and alpha ...
spanda38 0:8c52f4cf12bd 298
spanda38 0:8c52f4cf12bd 299 int k = (i + 1) % 4;
spanda38 0:8c52f4cf12bd 300 y[k][0] = y[i][0];
spanda38 0:8c52f4cf12bd 301 y[k][1] = y[i][1];
spanda38 0:8c52f4cf12bd 302 y[i][0] = dist;
spanda38 0:8c52f4cf12bd 303 y[i][1] = alpha; //Log in the data
spanda38 0:8c52f4cf12bd 304 i = k;
spanda38 0:8c52f4cf12bd 305
spanda38 0:8c52f4cf12bd 306 int w = (j + 1) % 2;
spanda38 0:8c52f4cf12bd 307 u[w][0] = u[j][0];
spanda38 0:8c52f4cf12bd 308 u[w][1] = u[j][1];
spanda38 0:8c52f4cf12bd 309 u[j][0] = dist;
spanda38 0:8c52f4cf12bd 310 u[j][1] = alpha; //Log in the data
spanda38 0:8c52f4cf12bd 311 j = w;
spanda38 0:8c52f4cf12bd 312
spanda38 0:8c52f4cf12bd 313
spanda38 0:8c52f4cf12bd 314 //Hybrid Control Algorithms ...
spanda38 0:8c52f4cf12bd 315
spanda38 0:8c52f4cf12bd 316 if(warning == 1)
spanda38 0:8c52f4cf12bd 317 {
spanda38 0:8c52f4cf12bd 318 ventCutOff();
spanda38 0:8c52f4cf12bd 319 } //Dont move if there is a contingency
spanda38 0:8c52f4cf12bd 320
spanda38 0:8c52f4cf12bd 321 if (warning == 0) {
spanda38 0:8c52f4cf12bd 322 //There is no obstacle
spanda38 0:8c52f4cf12bd 323 //Plow ahead at optimal speed
spanda38 0:8c52f4cf12bd 324 //propeller = 0.15; //Fill up the cushions
spanda38 0:8c52f4cf12bd 325 if (inflator == 0) {
spanda38 0:8c52f4cf12bd 326 ventFull();
spanda38 0:8c52f4cf12bd 327 }
spanda38 0:8c52f4cf12bd 328 rudder1 = 0.5; //Keep Rudder Positions Straight
spanda38 0:8c52f4cf12bd 329 deploy = 1;
spanda38 0:8c52f4cf12bd 330 //Move now
spanda38 0:8c52f4cf12bd 331 if (propeller != 0.15) {
spanda38 0:8c52f4cf12bd 332 if (propeller == 0) {
spanda38 0:8c52f4cf12bd 333 propeller = 1;
spanda38 0:8c52f4cf12bd 334 propeller = 0;
spanda38 0:8c52f4cf12bd 335 }
spanda38 0:8c52f4cf12bd 336 wait(0.5);
spanda38 0:8c52f4cf12bd 337 propeller = 0.15;
spanda38 0:8c52f4cf12bd 338 }
spanda38 0:8c52f4cf12bd 339 } if (dist > 100 && dist < 300 && old_dist > 100 && old_dist < 300) {
spanda38 0:8c52f4cf12bd 340 // Now you are in a situation where there is something ahead but not a danger to stop
spanda38 0:8c52f4cf12bd 341 // Rover should make some predictive driving and try to turn
spanda38 0:8c52f4cf12bd 342 float ref_y = dist + 50;
spanda38 0:8c52f4cf12bd 343 float ref_alpha = alpha; //You can change these!
spanda38 0:8c52f4cf12bd 344
spanda38 0:8c52f4cf12bd 345 float ref_F;
spanda38 0:8c52f4cf12bd 346 float ref_tau;
spanda38 0:8c52f4cf12bd 347 //Need to compute these and set these as the required angles and rotation RPMs to the controllers...
spanda38 0:8c52f4cf12bd 348 // You can change the parameters on top if needed! No problems with that...
spanda38 0:8c52f4cf12bd 349 // Get the x from there
spanda38 0:8c52f4cf12bd 350
spanda38 0:8c52f4cf12bd 351 x[0] = -8.5899 * ref_F + 8.5899 * y[2][0] - 8.5899 * y[3][0];
spanda38 0:8c52f4cf12bd 352 x[1] = 0.0307 * ref_F - 0.0234 * y[1][0] - 0.0140 * y[2][0];
spanda38 0:8c52f4cf12bd 353 x[2] = -8.5899 * ref_tau + 8.5899 * y[2][1] - 8.5899 * y[3][1];
spanda38 0:8c52f4cf12bd 354 x[3] = 0.0307 * ref_tau - 0.0234 * y[1][1] - 0.0140 * y[2][1]; // Least Sq Approx Solution BEST LINEAR UNBIASED ESTIMATOR
spanda38 0:8c52f4cf12bd 355 /*
spanda38 0:8c52f4cf12bd 356 -8.5899 0 0 0 8.5899 0 -8.5899 0
spanda38 0:8c52f4cf12bd 357 0.0307 0 -0.0234 0 -0.0140 0 0 0 = Observability Matrix Least Sq Approx Matrix
spanda38 0:8c52f4cf12bd 358 0 -8.5899 0 0 0 8.5899 0 -8.5899
spanda38 0:8c52f4cf12bd 359 0 0.0307 0 -0.0234 0 -0.0140 0 0
spanda38 0:8c52f4cf12bd 360
spanda38 0:8c52f4cf12bd 361 2.8091 0.0127 0 0
spanda38 0:8c52f4cf12bd 362 0 0 2.8091 0.0127
spanda38 0:8c52f4cf12bd 363 -9.1215 0.0602 0 0
spanda38 0:8c52f4cf12bd 364 0 0 -9.1215 0.0602 = Controllability Matrix Least Norm Matrix
spanda38 0:8c52f4cf12bd 365 -0.1843 0.0065 0 0
spanda38 0:8c52f4cf12bd 366 0 0 -0.1843 0.0065
spanda38 0:8c52f4cf12bd 367 6.4967 -0.0132 0 0
spanda38 0:8c52f4cf12bd 368 0 0 6.4967 -0.0132
spanda38 0:8c52f4cf12bd 369 */
spanda38 0:8c52f4cf12bd 370 ref_tau = 0.4967 * x[2] - 0.0132 * x[3];
spanda38 0:8c52f4cf12bd 371 ref_F = 6.4967 * x[0] - 0.0132 * x[1];
spanda38 0:8c52f4cf12bd 372 command(ref_tau, ref_F); //Computes the values and compensates...
spanda38 0:8c52f4cf12bd 373
spanda38 0:8c52f4cf12bd 374 ref_tau = 0 * x[0] + 0 * x[1] + (-0.01843) * x[2] + 0.0065 * x[3];
spanda38 0:8c52f4cf12bd 375 ref_F =-0.1843 * x[0] + 0.0065 * x[1] + 0 * x[2] + 0 * x[3];
spanda38 0:8c52f4cf12bd 376 command(ref_tau, ref_F);
spanda38 0:8c52f4cf12bd 377
spanda38 0:8c52f4cf12bd 378 ref_F = -9.1215 * x[0] + 0.0602 * x[1] + 0 * x[2] + 0 * x[3];
spanda38 0:8c52f4cf12bd 379 ref_tau = 0 * x[0] + 0 * x[1] + (-0.1215) * x[2] + 0.0602 * x[3];
spanda38 0:8c52f4cf12bd 380 command(ref_tau, ref_F);
spanda38 0:8c52f4cf12bd 381
spanda38 0:8c52f4cf12bd 382 ref_tau = 0 * x[0] + 0 * x[1] + 0.8091 * x[2] + 0.0127 * x[3];
spanda38 0:8c52f4cf12bd 383 ref_F = 2.8091 * x[0] + 0.0127 * x[1] + 0 * x[2] + 0 * x[3];
spanda38 0:8c52f4cf12bd 384 command(ref_tau, ref_F);
spanda38 0:8c52f4cf12bd 385 }
spanda38 0:8c52f4cf12bd 386 }
spanda38 0:8c52f4cf12bd 387 }