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
Show Stats 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 lastCorrectionTime = 0;
int lastPrintTime = 0; int lastPrintTime = 0;
static int fastTimer = 80; //ms static int fastTimer = 10; //ms
static int slowTimer = 1000; //ms static int slowTimer = 1000; //ms
//lqr stuff //lqr stuff
@ -171,7 +171,8 @@ void setSpeed(){
float speed_ms = rps * 0.05; float speed_ms = rps * 0.05;
float angle_r = angle_pitch_output * 0.318; float angle_r = angle_pitch_output * 0.318;
float angle_speed_rs = rps; 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); //speed = -22 * inputToControlSystem(0, 1);
if(speed<0){ if(speed<0){
digitalWrite(MotorPinB, CW); digitalWrite(MotorPinB, CW);
@ -184,7 +185,7 @@ void setSpeed(){
else { else {
speed = 0; speed = 0;
} }
speed = calc_speed(speed); if(speed!=0){ speed = constrain(speed, -255, 255);speed = calc_speed(speed); }
speed = abs(speed); speed = abs(speed);
speed = constrain(speed, 0, 255); speed = constrain(speed, 0, 255);
analogWrite(MotorSpeedB, speed); //Wheel close to connections 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[1] * speed_ms +
matrix_K[2] * angle_r + matrix_K[2] * angle_r +
matrix_K[3] * angle_speed_rs; matrix_K[3] * angle_speed_rs;
result *= 0.05;
/* /*
if (result > 0.29) { if (result > 0.29) {
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 { } else {
result = result; result = result;
}*/ }*/
Serial.print("K calculation: "); Serial.println(result); Serial.print("K calculation (force): "); Serial.println(result);
return result; return result;
} }
float calc_speed(float input) { float calc_speed(float input) {