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

testa bös

Browse Source
This commit is contained in:
Thefeli73 2021-04-19 17:14:25 +02:00
parent 1bf3f51783
commit e45732e5d1
2 changed files with 40 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EENX15_LQR/EENX15_LQR.ino
View File

@ -100,6 +100,7 @@ void setup() {
pinMode(MotorPinB, OUTPUT);
pinMode(MotorSpeedB, OUTPUT);
pinMode(MotorBrakeB, OUTPUT);
}
void loop() {
@ -148,9 +149,10 @@ void printInfo(){
void setSpeed(){
if(abs(safe_angle)<50 ){
//speed = 8*safe_angle;
float position_m = countA/174.76;
float position_m = -countA/174.76;
float angle_r = angle_pitch_output * 0.318;
speed = 22 * inputToControlSystem(position_m, angle_r);
speed = -22 * inputToControlSystem(position_m, angle_r);
//speed = -22 * inputToControlSystem(0, 1);
if(speed<0){
digitalWrite(MotorPinB, CW);
digitalWrite(MotorPinA, CCW);

47
EENX15_LQR/LQR.ino
View File

@ -8,12 +8,18 @@
// Expression: A
// Referenced by: '<Root>/Gain4'
const double Gain4_Gain [16] = { 0.0, 0.0, 0.0, 0.0, 1.0, -0.20780947085442231, 0.0, -0.52810302415000854, 0.0, 13.239785742831822, 0.0, 58.601480177829842, 0.0, 0.0, 1.0, 0.0 };
const double Gain4_Gain [16] = { 0.0, 0.0, 0.0, 0.0,
1.0, -0.20780947085442231, 0.0, -0.52810302415000854,
0.0, 13.239785742831822, 0.0, 58.601480177829842,
0.0, 0.0, 1.0, 0.0 };
//const double Gain4_Gain [16] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
// Expression: C
// Referenced by: '<Root>/Gain6'
const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
//const double Gain6_Gain [8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
// Expression: L
// Referenced by: '<Root>/Gain2'
@ -21,6 +27,9 @@ const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
const double Gain2_Gain [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
-115.34372132703447, -1.0534041975488044, -48.223441605702455,
117.16185100039935, 3490.0480780568214 };
//const double Gain2_Gain [8] = { 116.63033952875418, 338.78673967111704, -1.4473912197449676,
// -115.34372132703447, -1.0534041975488044, -48.223441605702455,
// 117.16185100039935, 34.900480780568214 };
// Expression: B
// Referenced by: '<Root>/Gain3'
@ -39,14 +48,14 @@ double Sum4[4];
// v ///////////////////////////////////
double tmp[2];
int rtb_Saturation;
double rtb_Saturation = 0.0;
// | ///////////////////////////////////
// | //Row 140-143 in Arduino_skal.cpp
// v ///////////////////////////////////
// Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas.
int saturatedSignalToMotors(){
double saturatedSignalToMotors(){
rtb_Saturation = ((-31.622776601683942 * Integrator1_CSTATE[0] +
-21.286439360075747 * Integrator1_CSTATE[1]) +
80.789376267003959 * Integrator1_CSTATE[2]) +
@ -59,12 +68,13 @@ int saturatedSignalToMotors(){
} else {
rtb_Saturation = 0.0 - rtb_Saturation;
}
Serial.print("Saturation = "); Serial.println(rtb_Saturation);
return rtb_Saturation;
}
// | ///////////////////////////////////
// | //Row 165-188 in Arduino_skal.cpp
// v ///////////////////////////////////
int inputToControlSystem(float position_m, float angle_r){
double inputToControlSystem(float position_m, float angle_r){
float posAndAng[] = {position_m, angle_r};
for (int i = 0; i < 2; i++) {
tmp[i] = posAndAng[i] - (((Gain6_Gain[i + 2] *
@ -90,7 +100,10 @@ int inputToControlSystem(float position_m, float angle_r){
(Gain4_Gain[i + 4] * Integrator1_CSTATE[1] +
Gain4_Gain[i] * Integrator1_CSTATE[0])));
}
Serial.print("Sum4 0 = "); Serial.println(Sum4[0]);
Serial.print("Sum4 1 = "); Serial.println(Sum4[1]);
Serial.print("Sum4 2 = "); Serial.println(Sum4[2]);
Serial.print("Sum4 3 = "); Serial.println(Sum4[3]);
Arduino_skal_derivatives();
return saturatedSignalToMotors();
}
@ -100,13 +113,25 @@ int inputToControlSystem(float position_m, float angle_r){
// v ///////////////////////////////////
void Arduino_skal_derivatives()
{/*
{
/*
XDot *_rtXdot;
_rtXdot = ((XDot *) (&rtM)->derivs);
// Derivatives for Integrator: '<Root>/Integrator1'
_rtXdot->Integrator1_CSTATE[0] = rtDW.Sum4[0];
_rtXdot->Integrator1_CSTATE[1] = rtDW.Sum4[1];
_rtXdot->Integrator1_CSTATE[2] = rtDW.Sum4[2];
_rtXdot->Integrator1_CSTATE[3] = rtDW.Sum4[3];*/
_rtXdot->Integrator1_CSTATE[0] = Sum4[0];
_rtXdot->Integrator1_CSTATE[1] = Sum4[1];
_rtXdot->Integrator1_CSTATE[2] = Sum4[2];
_rtXdot->Integrator1_CSTATE[3] = Sum4[3];
Integrator1_CSTATE[0] = Sum4[0] * (fastTimer/1000.0);
Integrator1_CSTATE[1] = Sum4[1] * (fastTimer/1000.0);
Integrator1_CSTATE[2] = Sum4[2] * (fastTimer/1000.0);
Integrator1_CSTATE[3] = Sum4[3] * (fastTimer/1000.0);
*/
// Derivatives for Integrator: '<Root>/Integrator1'
for (int i = 0; i < 4; i++) {
Integrator1_CSTATE[i] = Sum4[i] * 80/1000.0;
Serial.print("Integrator: "); Serial.println(Integrator1_CSTATE[i]);
}
}