remove code from temp lqr, remove unused lines

This commit is contained in:
Thefeli73 2021-04-15 16:55:16 +02:00
parent 0de3be94eb
commit aefed82ba4
2 changed files with 0 additions and 26 deletions

View File

@ -143,9 +143,6 @@ void printInfo(){
Serial.print("Rads rotated: "); Serial.println(countA/8.91); //ca. 56 tick per rotation, 6.26 rads per rotation Serial.print("Rads rotated: "); Serial.println(countA/8.91); //ca. 56 tick per rotation, 6.26 rads per rotation
Serial.print("RPM: "); Serial.println(rpm); //ca. 56 tick per rotation Serial.print("RPM: "); Serial.println(rpm); //ca. 56 tick per rotation
Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation
//Serial.print("A: "); Serial.println(countA);
//Serial.print("B: "); Serial.println(countB);
} }
void setSpeed(){ void setSpeed(){

View File

@ -1,23 +0,0 @@
/** Control Law*/
const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237};
//K from our calculation
//const float K[statesNumber] = {-31.6228, -21.2864, 80.7894, 13.4246};
/** 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;
}