From 487837cffae38c8037e6295f3734cf3faf77a0ef Mon Sep 17 00:00:00 2001 From: Thefeli73 Date: Wed, 12 May 2021 19:01:39 +0200 Subject: [PATCH] kodning som gjordes hos bragde --- EENX15_LQR/Arduino_skal.h | 228 +++++++++++++++++++++++++++++++++++ EENX15_LQR/EENX15_LQR.ino | 30 +++-- EENX15_LQR/LQR.ino | 1 - EENX15_LQR/lqr_fullstate.ino | 17 ++- 4 files changed, 257 insertions(+), 19 deletions(-) create mode 100644 EENX15_LQR/Arduino_skal.h diff --git a/EENX15_LQR/Arduino_skal.h b/EENX15_LQR/Arduino_skal.h new file mode 100644 index 0000000..07d5292 --- /dev/null +++ b/EENX15_LQR/Arduino_skal.h @@ -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 +#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 '' + struct DW { + real_T Sum4[4]; // '/Sum4' + }; + + // Continuous states (default storage) + struct X { + real_T Integrator1_CSTATE[4]; // '/Integrator1' + }; + + // State derivatives (default storage) + struct XDot { + real_T Integrator1_CSTATE[4]; // '/Integrator1' + }; + + // State disabled + struct XDis { + boolean_T Integrator1_CSTATE[4]; // '/Integrator1' + }; + + // Constant parameters (default storage) + struct ConstP { + // Expression: [100;200] + // Referenced by: '/vartejag' + + real_T vartejag_Value[2]; + + // Expression: A + // Referenced by: '/Gain4' + + real_T Gain4_Gain[16]; + + // Expression: C + // Referenced by: '/Gain6' + + real_T Gain6_Gain[8]; + + // Expression: L + // Referenced by: '/Gain2' + + real_T Gain2_Gain[8]; + + // Expression: B + // Referenced by: '/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 '/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 /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('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'Arduino_skal' + +#endif // RTW_HEADER_Arduino_skal_h_ + +// +// File trailer for generated code. +// +// [EOF] +// \ No newline at end of file diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino index 289a59d..b76c5cc 100644 --- a/EENX15_LQR/EENX15_LQR.ino +++ b/EENX15_LQR/EENX15_LQR.ino @@ -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 - } - else{ - speed = 0; - analogWrite(MotorSpeedB, speed); - analogWrite(MotorSpeedA, speed); + 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 } - Serial.print("Speed to motors: "); Serial.println(speed); + Serial.print("PWM to motors: "); Serial.println(PWM); } int directionA(){ if(digitalRead(encoderA2) == HIGH){ diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino index 897eafb..8e98a61 100644 --- a/EENX15_LQR/LQR.ino +++ b/EENX15_LQR/LQR.ino @@ -137,7 +137,6 @@ void Arduino_skal_derivatives() _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); diff --git a/EENX15_LQR/lqr_fullstate.ino b/EENX15_LQR/lqr_fullstate.ino index 34dd634..afc6b60 100644 --- a/EENX15_LQR/lqr_fullstate.ino +++ b/EENX15_LQR/lqr_fullstate.ino @@ -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; }