schulze/EENX15
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixes, idk

Browse Source
This commit is contained in:
Thefeli73
2021-04-24 11:15:41 +02:00
parent 5dad9a6f6d
commit b57bd6ca27
2 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
EENX15_LQR/EENX15_LQR.ino
View File

@@ -3,7 +3,7 @@
int lastCorrectionTime = 0;
int lastPrintTime = 0;
static int fastTimer = 80; //ms
static int fastTimer = 10; //ms
static int slowTimer = 1000; //ms
//lqr stuff
@@ -171,7 +171,8 @@ void setSpeed(){
float speed_ms = rps * 0.05;
float angle_r = angle_pitch_output * 0.318;
float angle_speed_rs = rps;
speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs)/ 0.022745; // (0.29/0.05)/255
//speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs);/// 0.019608; // (0.20*255)
speed = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
//speed = -22 * inputToControlSystem(0, 1);
if(speed<0){
digitalWrite(MotorPinB, CW);
@@ -184,7 +185,7 @@ void setSpeed(){
else {
speed = 0;
}
speed = calc_speed(speed);
if(speed!=0){ speed = constrain(speed, -255, 255);speed = calc_speed(speed); }
speed = abs(speed);
speed = constrain(speed, 0, 255);
analogWrite(MotorSpeedB, speed); //Wheel close to connections

3
EENX15_LQR/lqr_fullstate.ino
View File

@@ -15,7 +15,6 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
matrix_K[1] * speed_ms +
matrix_K[2] * angle_r +
matrix_K[3] * angle_speed_rs;
result *= 0.05;
/*
if (result > 0.29) {
result = 0.29;
@@ -24,7 +23,7 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
} else {
result = result;
}*/
Serial.print("K calculation: "); Serial.println(result);
Serial.print("K calculation (force): "); Serial.println(result);
return result;
}
float calc_speed(float input) {