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

able to compile. change variable names, stop using some constructs, change types etc

Browse Source
This commit is contained in:
Thefeli73 2021-04-17 13:22:12 +02:00
parent 6f7ba57f82
commit cf17affee4
2 changed files with 27 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EENX15_LQR/Arduino_skal.h
View File

@ -156,7 +156,7 @@ class Arduino_skalModelClass {
uint32_T clockTick0; uint32_T clockTick0;
time_T stepSize0; time_T stepSize0;
uint32_T clockTick1; uint32_T clockTick1;
SimTimeStep simTimeStep; // SimTimeStep simTimeStep;
boolean_T stopRequestedFlag; boolean_T stopRequestedFlag;
time_T *t; time_T *t;
time_T tArray[2]; time_T tArray[2];

61
EENX15_LQR/LQR.ino
View File

@ -4,50 +4,41 @@
// | /////////////////////////////////// // | ///////////////////////////////////
// | //Row 24-52 in Arduino_skal_data.cpp // | //Row 24-52 in Arduino_skal_data.cpp
// v /////////////////////////////////// // v ///////////////////////////////////
const double Arduino_skalModelClass::ConstP rtConstP = {
// Expression: [100;200]
// Referenced by: '<Root>/vartejag'
{ 100.0, 200.0 },
// Expression: A // Expression: A
// Referenced by: '<Root>/Gain4' // Referenced by: '<Root>/Gain4'
{ 0.0, 0.0, 0.0, 0.0, 1.0, -0.20780947085442231, 0.0, -0.52810302415000854, 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 };
0.0, 13.239785742831822, 0.0, 58.601480177829842, 0.0, 0.0, 1.0, 0.0 },
// Expression: C // Expression: C
// Referenced by: '<Root>/Gain6' // Referenced by: '<Root>/Gain6'
{ 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }, const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
// Expression: L // Expression: L
// Referenced by: '<Root>/Gain2' // Referenced by: '<Root>/Gain2'
{ 116.63033952875418, 3387.8673967111704, -1.4473912197449676, const double Gain2_Gain [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
-115.34372132703447, -1.0534041975488044, -48.223441605702455, -115.34372132703447, -1.0534041975488044, -48.223441605702455,
117.16185100039935, 3490.0480780568214 }, 117.16185100039935, 3490.0480780568214 };
// Expression: B // Expression: B
// Referenced by: '<Root>/Gain3' // Referenced by: '<Root>/Gain3'
{ 0.0, 2.078094708544223, 0.0, 5.2810302415000852 } const double Gain3_Gain [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 };
};
// | /////////////////////////////////// // | ///////////////////////////////////
// | //Row 261-264 in Arduino_skal.cpp // | //Row 261-264 in Arduino_skal.cpp
// v /////////////////////////////////// // v ///////////////////////////////////
double Integrator1_CSTATE [4] = {0.0, 0.0, 0.0, 0.0};
rtX.Integrator1_CSTATE[0] = 0.0; double Sum4[4];
rtX.Integrator1_CSTATE[1] = 0.0;
rtX.Integrator1_CSTATE[2] = 0.0;
rtX.Integrator1_CSTATE[3] = 0.0;
// | /////////////////////////////////// // | ///////////////////////////////////
// | //Row 123-124 in Arduino_skal.cpp // | //Row 123-124 in Arduino_skal.cpp
// v /////////////////////////////////// // v ///////////////////////////////////
real_T tmp[2]; double tmp[2];
int rtb_Saturation; int rtb_Saturation;
// | /////////////////////////////////// // | ///////////////////////////////////
@ -56,10 +47,10 @@ int rtb_Saturation;
// Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas. // Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas.
int saturatedSignalToMotors(){ int saturatedSignalToMotors(){
rtb_Saturation = ((-31.622776601683942 * rtX.Integrator1_CSTATE[0] + rtb_Saturation = ((-31.622776601683942 * Integrator1_CSTATE[0] +
-21.286439360075747 * rtX.Integrator1_CSTATE[1]) + -21.286439360075747 * Integrator1_CSTATE[1]) +
80.789376267003959 * rtX.Integrator1_CSTATE[2]) + 80.789376267003959 * Integrator1_CSTATE[2]) +
13.42463576551093 * rtX.Integrator1_CSTATE[3]; 13.42463576551093 * Integrator1_CSTATE[3];
if (0.0 - rtb_Saturation > 11.5) { if (0.0 - rtb_Saturation > 11.5) {
rtb_Saturation = 11.5; rtb_Saturation = 11.5;
@ -75,16 +66,16 @@ int saturatedSignalToMotors(){
// v /////////////////////////////////// // v ///////////////////////////////////
int inputToControlSystem(float position_m, float angle_r){ int inputToControlSystem(float position_m, float angle_r){
float posAndAng[] = {position_m, angle_r}; float posAndAng[] = {position_m, angle_r};
for (i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
tmp[i] = rtConstP.posAndAng[i] - (((rtConstP.Gain6_Gain[i + 2] * tmp[i] = posAndAng[i] - (((Gain6_Gain[i + 2] *
rtX.Integrator1_CSTATE[1] + rtConstP.Gain6_Gain[i] * Integrator1_CSTATE[1] + Gain6_Gain[i] *
rtX.Integrator1_CSTATE[0]) + rtConstP.Gain6_Gain[i + 4] * Integrator1_CSTATE[0]) + Gain6_Gain[i + 4] *
rtX.Integrator1_CSTATE[2]) + rtConstP.Gain6_Gain[i + 6] * Integrator1_CSTATE[2]) + Gain6_Gain[i + 6] *
rtX.Integrator1_CSTATE[3]); Integrator1_CSTATE[3]);
} }
// End of Sum: '<Root>/Sum2' // End of Sum: '<Root>/Sum2'
for (i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
// Sum: '<Root>/Sum4' incorporates: // Sum: '<Root>/Sum4' incorporates:
// Gain: '<Root>/Gain2' // Gain: '<Root>/Gain2'
// Gain: '<Root>/Gain3' // Gain: '<Root>/Gain3'
@ -92,12 +83,12 @@ int inputToControlSystem(float position_m, float angle_r){
// Integrator: '<Root>/Integrator1' // Integrator: '<Root>/Integrator1'
// Sum: '<Root>/Sum3' // Sum: '<Root>/Sum3'
rtDW.Sum4[i] = ((rtConstP.Gain2_Gain[i + 4] * tmp[1] + rtConstP.Gain2_Gain[i] Sum4[i] = ((Gain2_Gain[i + 4] * tmp[1] + Gain2_Gain[i]
* tmp[0]) + rtConstP.Gain3_Gain[i] * rtb_Saturation) + * tmp[0]) + Gain3_Gain[i] * rtb_Saturation) +
(rtConstP.Gain4_Gain[i + 12] * rtX.Integrator1_CSTATE[3] + (Gain4_Gain[i + 12] * Integrator1_CSTATE[3] +
(rtConstP.Gain4_Gain[i + 8] * rtX.Integrator1_CSTATE[2] + (Gain4_Gain[i + 8] * Integrator1_CSTATE[2] +
(rtConstP.Gain4_Gain[i + 4] * rtX.Integrator1_CSTATE[1] + (Gain4_Gain[i + 4] * Integrator1_CSTATE[1] +
rtConstP.Gain4_Gain[i] * rtX.Integrator1_CSTATE[0]))); Gain4_Gain[i] * Integrator1_CSTATE[0])));
} }
/* /*
Integrator1_CSTATE[0] = rtDW.Sum4[0]; Integrator1_CSTATE[0] = rtDW.Sum4[0];