2021-04-13 21:44:53 +02:00
|
|
|
/** Control Law*/
|
|
|
|
|
const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237};
|
2021-04-13 23:43:54 +02:00
|
|
|
|
|
|
|
|
//K from our calculation
|
|
|
|
|
//const float K[statesNumber] = {-31.6228, -21.2864, 80.7894, 13.4246};
|
|
|
|
|
|
2021-04-13 21:44:53 +02:00
|
|
|
/** Correction factor */
|
|
|
|
|
const float scaleConst = 2.5;
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* LQR control law
|
|
|
|
|
*/
|
|
|
|
|
void lqr() {
|
|
|
|
|
speed = 0;
|
|
|
|
|
|
|
|
|
|
speed -= motorAngularPosition * K[0];
|
|
|
|
|
speed -= correctedAngularPosition * K[1];
|
|
|
|
|
speed -= motorAngularSpeed * K[2];
|
|
|
|
|
speed -= angularSpeed * K[3];
|
|
|
|
|
|
|
|
|
|
speed = speed*scaleConst;
|
|
|
|
|
|
|
|
|
|
}
|
