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

kodning som gjordes hos bragde

Browse Source
This commit is contained in:
Thefeli73 2021-05-12 19:01:39 +02:00
parent b57bd6ca27
commit 487837cffa
4 changed files with 257 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
EENX15_LQR/Arduino_skal.h Normal file
View File

@ -0,0 +1,228 @@
//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only. Not for
// government, commercial, or other organizational use.
//
// File: Arduino_skal.h
//
// Code generated for Simulink model 'Arduino_skal'.
//
// Model version : 1.1
// Simulink Coder version : 9.5 (R2021a) 14-Nov-2020
// C/C++ source code generated on : Thu Apr 15 22:06:00 2021
//
// Target selection: ert.tlc
// Embedded hardware selection: AMD->x86-64 (Windows64)
// Code generation objectives:
// 1. Execution efficiency
// 2. RAM efficiency
// Validation result: Not run
//
#ifndef RTW_HEADER_Arduino_skal_h_
#define RTW_HEADER_Arduino_skal_h_
//#include <cstring>
#include "rtwtypes.h"
//#include "rtw_continuous.h"
//#include "rtw_solver.h"
// Model Code Variants
// Macros for accessing real-time model data structure
#ifndef rtmGetErrorStatus
#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus)
#endif
#ifndef rtmSetErrorStatus
#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val))
#endif
#ifndef rtmGetStopRequested
#define rtmGetStopRequested(rtm) ((rtm)->Timing.stopRequestedFlag)
#endif
#ifndef rtmSetStopRequested
#define rtmSetStopRequested(rtm, val) ((rtm)->Timing.stopRequestedFlag = (val))
#endif
#ifndef rtmGetStopRequestedPtr
#define rtmGetStopRequestedPtr(rtm) (&((rtm)->Timing.stopRequestedFlag))
#endif
#ifndef rtmGetT
#define rtmGetT(rtm) (rtmGetTPtr((rtm))[0])
#endif
#ifndef rtmGetTPtr
#define rtmGetTPtr(rtm) ((rtm)->Timing.t)
#endif
#ifndef ODE3_INTG
#define ODE3_INTG
// ODE3 Integration Data
struct ODE3_IntgData {
real_T *y; // output
real_T *f[3]; // derivatives
};
#endif
// Class declaration for model Arduino_skal
class Arduino_skalModelClass {
// public data and function members
public:
// Block signals and states (default storage) for system '<Root>'
struct DW {
real_T Sum4[4]; // '<Root>/Sum4'
};
// Continuous states (default storage)
struct X {
real_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
};
// State derivatives (default storage)
struct XDot {
real_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
};
// State disabled
struct XDis {
boolean_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
};
// Constant parameters (default storage)
struct ConstP {
// Expression: [100;200]
// Referenced by: '<Root>/vartejag'
real_T vartejag_Value[2];
// Expression: A
// Referenced by: '<Root>/Gain4'
real_T Gain4_Gain[16];
// Expression: C
// Referenced by: '<Root>/Gain6'
real_T Gain6_Gain[8];
// Expression: L
// Referenced by: '<Root>/Gain2'
real_T Gain2_Gain[8];
// Expression: B
// Referenced by: '<Root>/Gain3'
real_T Gain3_Gain[4];
};
// Real-time Model Data Structure
struct RT_MODEL {
const char_T *errorStatus;
//RTWSolverInfo solverInfo;
X *contStates;
int_T *periodicContStateIndices;
real_T *periodicContStateRanges;
real_T *derivs;
boolean_T *contStateDisabled;
boolean_T zCCacheNeedsReset;
boolean_T derivCacheNeedsReset;
boolean_T CTOutputIncnstWithState;
real_T odeY[4];
real_T odeF[3][4];
ODE3_IntgData intgData;
//
// Sizes:
// The following substructure contains sizes information
// for many of the model attributes such as inputs, outputs,
// dwork, sample times, etc.
struct {
int_T numContStates;
int_T numPeriodicContStates;
int_T numSampTimes;
} Sizes;
//
// Timing:
// The following substructure contains information regarding
// the timing information for the model.
struct {
uint32_T clockTick0;
time_T stepSize0;
uint32_T clockTick1;
SimTimeStep simTimeStep;
boolean_T stopRequestedFlag;
time_T *t;
time_T tArray[2];
} Timing;
};
// model initialize function
void initialize();
// model step function
void step();
// Constructor
Arduino_skalModelClass();
// Destructor
~Arduino_skalModelClass();
// Real-Time Model get method
Arduino_skalModelClass::RT_MODEL * getRTM();
// private data and function members
private:
// Block signals and states
DW rtDW;
X rtX; // Block continuous states
// Real-Time Model
RT_MODEL rtM;
// Continuous states update member function
//void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si );
// Derivatives member function
void Arduino_skal_derivatives();
};
// Constant parameters (default storage)
extern const Arduino_skalModelClass::ConstP rtConstP;
//-
// These blocks were eliminated from the model due to optimizations:
//
// Block '<Root>/Kr' : Eliminated nontunable gain of 1
//-
// The generated code includes comments that allow you to trace directly
// back to the appropriate location in the model. The basic format
// is <system>/block_name, where system is the system number (uniquely
// assigned by Simulink) and block_name is the name of the block.
//
// Use the MATLAB hilite_system command to trace the generated code back
// to the model. For example,
//
// hilite_system('<S3>') - opens system 3
// hilite_system('<S3>/Kp') - opens and selects block Kp which resides in S3
//
// Here is the system hierarchy for this model
//
// '<Root>' : 'Arduino_skal'
#endif // RTW_HEADER_Arduino_skal_h_
//
// File trailer for generated code.
//
// [EOF]
//

30
EENX15_LQR/EENX15_LQR.ino
View File

@ -23,7 +23,10 @@ float motorAngularSpeed = 0;
/** PWM signal applied to the motor's driver 255 is 100% */
int speed;
int Va;
int safe_angle;
float force;
int PWM;
//gyro stuff
@ -160,7 +163,6 @@ void set_test_speed(){
analogWrite(MotorSpeedA, speed); //First experiment wheel
Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation
Serial.print("Speed: "); Serial.println(speed); //ca. 56 tick per rotation, 6.26 rads per rotation
temp_counter += 3;
}
*/
@ -172,31 +174,27 @@ void setSpeed(){
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.019608; // (0.20*255)
speed = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
force = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
//speed = -22 * inputToControlSystem(0, 1);
if(speed<0){
if(force<0){
digitalWrite(MotorPinB, CW);
digitalWrite(MotorPinA, CCW);
}
else if(speed>0){
else if(force>0){
digitalWrite(MotorPinB, CCW);
digitalWrite(MotorPinA, CW);
}
else {
speed = 0;
force = 0;
}
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
analogWrite(MotorSpeedA, speed); //First experiment wheel
if(force!=0){Va = calc_speed(force, angle_speed_rs); }
Va = abs(Va);
PWM = 255*Va/12;
PWM = constrain(PWM, 0, 255);
analogWrite(MotorSpeedB, PWM); //Wheel close to connections
analogWrite(MotorSpeedA, PWM); //First experiment wheel
}
else{
speed = 0;
analogWrite(MotorSpeedB, speed);
analogWrite(MotorSpeedA, speed);
}
Serial.print("Speed to motors: "); Serial.println(speed);
Serial.print("PWM to motors: "); Serial.println(PWM);
}
int directionA(){
if(digitalRead(encoderA2) == HIGH){

1
EENX15_LQR/LQR.ino
View File

@ -138,7 +138,6 @@ void Arduino_skal_derivatives()
_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);

17
EENX15_LQR/lqr_fullstate.ino
View File

@ -8,7 +8,7 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
// const float matrix_K [4] = {-0.1054, -0.5273, 23.5546, 3.1876};
// const float matrix_K [4] = {-0.7071, -1.5720, 26.0726, 3.7050};
// const float matrix_K [4] = {-0.7071, -1.5980, 26.6081, 4.3220};
const float matrix_K [4] = {-0.7071, -1.7751, 34.5368, 4.8793};
const float matrix_K [4] = {-0.7071, -1.7751, 34.5368, 4.8793};
float result;
result = matrix_K[0] * position_m +
@ -26,13 +26,15 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
Serial.print("K calculation (force): "); Serial.println(result);
return result;
}
float calc_speed(float input) {
float calc_speed(float input, float angle_speed_rs) {
/*
float a = -2971;
float b = -0.9929;
float c = 90.75;
float radps = a * pow(speed, b) + c; ////// the response graph
*/
/*
float scale = 1.5;
input = abs(input)*0.30796; // scale down to rad/s (78,53/255)
Serial.print("input: "); Serial.println(input);
@ -40,5 +42,16 @@ float calc_speed(float input) {
result *= scale;
Serial.print("calcspeed: "); Serial.println(result);
return result;
*/
float I = (1/3)*1.74;
float km = 0.91*0.01;
float ke = 8.68*0.001*2*PI/60;
float Ir = I;
float Omega= angle_speed_rs;
float result = (km*ke/(Ir*5-km))*Omega;
Serial.print("RESULT");
Serial.print(result);
return result;
}