Commit f9178a46 authored by Patrick Göttsch's avatar Patrick Göttsch
Browse files

Gitlab Runner Update from cstd-aux-files/master:Adds auto upload of exercise...

Gitlab Runner Update from cstd-aux-files/master:Adds auto upload of exercise files to public repo for CSTD
parent 964b1f59

Too many changes to show.

To preserve performance only 1000 of 1000+ files are displayed.
close all;
%% Robot Parameters
% Constants
g = 9.81; %gravity - m/s^2
% Motor
R = 4.5; %armature resistance - Ohms
kb = 0.495; %back EMF coeficient - V*seg/rad
kt = 0.470; %motor torque constant - Nm/A
% Wheel
mw = 0.0183; %mass of one wheel - kg
Jw = 7.462e-06; %moment of intertia - kg*m^2
r = 0.0216; %radius - m
% Segway
mp = 0.3723; %mass of board + other components - kg
Jp = 0.00467; %moment of intertia - kg*m^2
l = 0.112; %length to center - m
% linear State Space Model
m_eq = 2*mw + mp + 2*Jw/(r.^2);
J_eq = Jp + mp*l^2;
detM = m_eq*J_eq - (mp*l)^2;
a11 = -1*kt*kb/(R*r*detM) * (J_eq/r + mp*l);
a12 = 1*kt*kb/(R*r*detM) * (J_eq + mp*l*r);
a21 = 1*kt*kb/(R*r*detM) * (m_eq + mp*l/r);
a22 = -1*kt*kb/(R*r*detM) * (m_eq*r + mp*l);
a31 = -mp*g*l/detM * mp*l;
a32 = mp*g*l/detM * m_eq;
b1 = kt/(R*r*detM) * (J_eq + mp*l*r);
b2 = -1*kt/(R*r*detM) * (m_eq*r + mp*l);
A = [0 0 1 0; 0 0 0 1; 0 a31 a11 a12; 0 a32 a21 a22];
B = [0; 0; b1; b2];
C = eye(4);
D = zeros(4,1);
close all;
% Model Verification
x_s = [0; 0; 0; 0]; %equilibrium state vector
[A_nl, B_nl, C_nl, D_nl] = linmod('nonlinear_model', x_s);
alpha0 = 5; %initial angle in degrees
x0 = [0; alpha0*pi/180; 0; 0]; %initial condition vector
K_LQR = [26.4575, 82.3959, 56.2528, 12.0057]; %LQR gain
Vs = 7.2; %saturation voltage
%% LQR Simulation
sim('simulation_LQR.slx');
while(~exist('simulation_LQR'))
pause(1);
end;
% linear Model
t_LIN = simout_LQR.Time;
x_LIN = simout_LQR.Data(:,1);
alpha_LIN = simout_LQR.Data(:,2);
u_LIN = simout_LQR.Data(:,5);
% nonlinear Model
t_NL = simout_LQR_nl.Time;
x_NL = simout_LQR_nl.Data(:,1);
alpha_NL = simout_LQR_nl.Data(:,2);
u_NL = simout_LQR_nl.Data(:,5);
figure;
subplot(3,1,1);
hold on;
grid on;
plot(t_LIN, x_LIN, 'r', 'LineWidth', 1.6);
plot(t_NL, x_NL, 'b', 'LineWidth', 1.2);
legend('linear', 'nonlinear', 'Location', 'NorthEast');
ylabel('$s(t)$', 'interpreter', 'latex');
xlim([0 2.5]);
subplot(3,1,2);
hold on;
grid on;
plot(t_LIN, alpha_LIN, 'r', 'LineWidth', 1.6);
plot(t_NL, alpha_NL, 'b', 'LineWidth', 1.2);
legend('linear', 'nonlinear', 'Location', 'NorthEast');
ylabel('$\alpha(t)$', 'interpreter', 'latex');
xlim([0 2.5]);
subplot(3,1,3);
hold on;
grid on;
plot(t_LIN, u_LIN, 'r', 'LineWidth', 1.6);
plot(t_NL, u_NL, 'b', 'LineWidth', 1.2);
legend('linear', 'nonlinear', 'Location', 'NorthEast');
ylabel('$u(t)$', 'interpreter', 'latex');
xlabel('Time [s]', 'interpreter', 'latex')
title(['$u_{max}(t)$ = ', num2str(max(abs([u_LIN; u_NL]))), ' V'], 'interpreter', 'latex');
xlim([0 2.5]);
11-11-2016
- lookup option for motor drivers to correct nonlinearities in output (common of darling type h bridges)
- encoders are hard coded to specific pins to allow direct reading of the pin port. DigitalRead() is significantly slower (20-30x) and can cause microprocessor satureation for high resolution encoders or fast motors
- PWM frequency selection is enabled on the encoder blocks
- Driver block is simplified and improved for faster execution
- Driver blocks have all been modified so a positive voltage produces a positive direction from the encoders
- SerialPlot.m is modified to scale correctly for numbers less than 1. Graphing and autoscaling has been improved.
V1.08312016.2015a
Changed SerialPlot.ma
- added automatic autoscale up to the full window witdth, then it must be manual autoscaled with the button
- removed the popup about trying to connect to the com port
- the for loop now adjust the number of samples read/displayed so it refreshes approximately 50 times a second
Removed M1V3 and MinsegMegaV3 since they are not currently made
replaces PWM44, PWM45, PWM46 Sfunctions from MinSegMega Library with the default Arduino PWM Block
removed the "signed PWM" blocks from library since they are redundant with the Motor Driver blocks (same except for a gain to voltage)
Transition from S-Funcitons to System Objects
V1.02032016.2015a
Important: The file/folder structure has changed - this is to begin support of matlab objects to replace s-functions for more universal support (mac)
currently the root folder 'RASPlib', and the and the 'sfunction_blocks' folder are are that are required. the 'blocks', 'src', and 'include' folders are in support of new implementaiton and may be deleted by the user if desired.
Users interested in the new object based version, or mac users who wish to test this functinality and report back are welcome to try the
'MinSegShield_Demo_M1V4_15a.slx' demo which uses the object based version of some of the s-functions.
Updated library and links for 2015a - oldver versions of simulink the library might have components/block move or become unorganized - it should not affect functinality but visual arrangement of the blocks might be moved
Multiple libray changes have taken place to fix bugs, functinality usability of several blocks. Apologies for a lack of a list of specific changes.
V1.01
SerialPlot.m - modified to automatically adjust the number of samples read so the refresh rate of the graph is 30hz.
V1.1
Added some missing mex files for various blocks
\ No newline at end of file
cd .
if "%1"=="" ("C:\PROGRA~1\MATLAB\R2017a\bin\win64\gmake" -f Experiment_LQR.mk all) else ("C:\PROGRA~1\MATLAB\R2017a\bin\win64\gmake" -f Experiment_LQR.mk %1)
@if errorlevel 1 goto error_exit
exit /B 0
:error_exit
echo The make command returned an error of %errorlevel%
An_error_occurred_during_the_call_to_make
/*
* Experiment_LQR.c
*
* Academic Student License -- for use by students to meet course
* requirements and perform academic research at degree granting
* institutions only. Not for government, commercial, or other
* organizational use.
*
* Code generation for model "Experiment_LQR".
*
* Model version : 1.2
* Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
* C source code generated on : Sun Sep 10 13:45:47 2017
*
* Target selection: ert.tlc
* Note: GRT includes extra infrastructure and instrumentation for prototyping
* Embedded hardware selection: Atmel->AVR
* Code generation objectives: Unspecified
* Validation result: Not run
*/
#include "Experiment_LQR.h"
#include "Experiment_LQR_private.h"
#include "Experiment_LQR_dt.h"
#define Experiment_LQR_DLPFmode (4.0)
#define Experiment_LQR_Encoder (1.0)
#define Experiment_LQR_PWMFSelect (1.0)
#define Experiment_LQR_PWMTimer (3.0)
#define Experiment_LQR_PinA (19.0)
#define Experiment_LQR_PinB (18.0)
#define Experiment_LQR_PinNumber (6.0)
/* Block signals (auto storage) */
B_Experiment_LQR_T Experiment_LQR_B;
/* Block states (auto storage) */
DW_Experiment_LQR_T Experiment_LQR_DW;
/* Real-time model */
RT_MODEL_Experiment_LQR_T Experiment_LQR_M_;
RT_MODEL_Experiment_LQR_T *const Experiment_LQR_M = &Experiment_LQR_M_;
real_T rt_atan2d_snf(real_T u0, real_T u1)
{
real_T y;
int16_T u0_0;
int16_T u1_0;
if (rtIsNaN(u0) || rtIsNaN(u1)) {
y = (rtNaN);
} else if (rtIsInf(u0) && rtIsInf(u1)) {
if (u0 > 0.0) {
u0_0 = 1;
} else {
u0_0 = -1;
}
if (u1 > 0.0) {
u1_0 = 1;
} else {
u1_0 = -1;
}
y = atan2(u0_0, u1_0);
} else if (u1 == 0.0) {
if (u0 > 0.0) {
y = RT_PI / 2.0;
} else if (u0 < 0.0) {
y = -(RT_PI / 2.0);
} else {
y = 0.0;
}
} else {
y = atan2(u0, u1);
}
return y;
}
/* Model step function */
void Experiment_LQR_step(void)
{
/* local block i/o variables */
real_T rtb_TSamp;
int32_T rtb_M2V3LeftConnector1819_0;
int16_T out[3];
real_T rtb_convertoradianssec;
real_T rtb_Sum6;
real_T rtb_WheelRaduis2;
real_T rtb_Product1;
real32_T tmp;
/* MATLABSystem: '<S8>/M2V3 Left Connector 18, 19' incorporates:
* Start for MATLABSystem: '<S8>/M2V3 Left Connector 18, 19'
*/
rtb_M2V3LeftConnector1819_0 = enc_output(Experiment_LQR_Encoder);
/* Gain: '<S8>/convert to radians1' incorporates:
* DataTypeConversion: '<S8>/Data Type Conversion3'
* MATLABSystem: '<S8>/M2V3 Left Connector 18, 19'
*/
Experiment_LQR_B.converttoradians1 = -Experiment_LQR_P.ES * (real_T)
rtb_M2V3LeftConnector1819_0;
/* Gain: '<S8>/Wheel Raduis2' */
rtb_WheelRaduis2 = -Experiment_LQR_P.Rw * Experiment_LQR_B.converttoradians1;
/* Start for MATLABSystem: '<S9>/Accelerometer' incorporates:
* MATLABSystem: '<S9>/Accelerometer'
*/
MPU6050Accel_Read(out);
/* Sum: '<S11>/Sum' incorporates:
* Constant: '<S11>/Z Bias'
* MATLABSystem: '<S9>/Accelerometer'
* Start for MATLABSystem: '<S9>/Accelerometer'
*/
Experiment_LQR_B.Sum = (real_T)out[2] - Experiment_LQR_P.z_mean;
/* Sum: '<S11>/Sum1' incorporates:
* Constant: '<S11>/Y Bias'
* MATLABSystem: '<S9>/Accelerometer'
* Start for MATLABSystem: '<S9>/Accelerometer'
*/
Experiment_LQR_B.Sum1 = (real_T)out[1] - (16384.0 + Experiment_LQR_P.y_mean);
/* Trigonometry: '<S11>/Trigonometric Function' incorporates:
* Gain: '<S11>/Gain2'
* Gain: '<S11>/Gain5'
*/
Experiment_LQR_B.TrigonometricFunction = rt_atan2d_snf
(Experiment_LQR_P.Gain2_Gain * Experiment_LQR_B.Sum,
Experiment_LQR_P.Gain5_Gain * Experiment_LQR_B.Sum1);
/* Start for MATLABSystem: '<S9>/Gyroscope' incorporates:
* MATLABSystem: '<S9>/Gyroscope'
*/
MPU6050Gyro_Read(out);
/* Gain: '<S9>/conver to radians//sec' incorporates:
* Constant: '<S9>/bias'
* DataTypeConversion: '<S9>/Data Type Conversion1'
* MATLABSystem: '<S9>/Gyroscope'
* Start for MATLABSystem: '<S9>/Gyroscope'
* Sum: '<S9>/Sum'
*/
rtb_convertoradianssec = ((real_T)out[0] - Experiment_LQR_P.x_mean) *
Experiment_LQR_P.GS;
/* Delay: '<S12>/Delay' */
Experiment_LQR_B.Delay = Experiment_LQR_DW.Delay_DSTATE;
/* Sum: '<S12>/Sum3' incorporates:
* Constant: '<S9>/Solver Step size'
* Gain: '<S12>/Gain3'
* Gain: '<S12>/Gain4'
* Gain: '<S9>/Gain5'
* Product: '<S12>/Product'
* Sum: '<S12>/Sum4'
*/
Experiment_LQR_B.Sum3 = (rtb_convertoradianssec * Experiment_LQR_P.Ts +
Experiment_LQR_B.Delay) * Experiment_LQR_P.Gain3_Gain +
Experiment_LQR_P.Gain5_Gain_f * Experiment_LQR_B.TrigonometricFunction *
Experiment_LQR_P.Gain4_Gain;
/* Sum: '<S9>/Sum6' incorporates:
* Constant: '<S9>/Constant1'
*/
rtb_Sum6 = Experiment_LQR_B.Sum3 - Experiment_LQR_P.Constant1_Value;
/* SampleTimeMath: '<S10>/TSamp'
*
* About '<S10>/TSamp':
* y = u * K where K = 1 / ( w * Ts )
*/
rtb_TSamp = Experiment_LQR_B.converttoradians1 * Experiment_LQR_P.TSamp_WtEt;
/* Gain: '<S8>/Wheel Raduis' incorporates:
* Sum: '<S10>/Diff'
* UnitDelay: '<S10>/UD'
*/
Experiment_LQR_B.WheelRaduis = (rtb_TSamp - Experiment_LQR_DW.UD_DSTATE) *
-Experiment_LQR_P.Rw;
/* Sin: '<S2>/Sine Wave' */
Experiment_LQR_B.SineWave = sin(Experiment_LQR_P.SineWave_Freq *
Experiment_LQR_M->Timing.t[0] + Experiment_LQR_P.SineWave_Phase) *
Experiment_LQR_P.SineWave_Amp + Experiment_LQR_P.SineWave_Bias;
/* Sum: '<Root>/Sum' incorporates:
* Gain: '<S2>/X_ref'
* Gain: '<S2>/X_ref1'
*/
Experiment_LQR_B.Sum_b[0] = rtb_WheelRaduis2 - Experiment_LQR_P.X_ref_Gain[0] *
Experiment_LQR_B.SineWave * Experiment_LQR_P.sinewave;
Experiment_LQR_B.Sum_b[1] = rtb_Sum6 - Experiment_LQR_P.X_ref_Gain[1] *
Experiment_LQR_B.SineWave * Experiment_LQR_P.sinewave;
Experiment_LQR_B.Sum_b[2] = Experiment_LQR_B.WheelRaduis -
Experiment_LQR_P.X_ref_Gain[2] * Experiment_LQR_B.SineWave *
Experiment_LQR_P.sinewave;
Experiment_LQR_B.Sum_b[3] = rtb_convertoradianssec -
Experiment_LQR_P.X_ref_Gain[3] * Experiment_LQR_B.SineWave *
Experiment_LQR_P.sinewave;
/* Product: '<Root>/Product1' incorporates:
* Fcn: '<Root>/Fcn'
* Gain: '<Root>/LQR Controller'
* Gain: '<Root>/alpha'
*/
rtb_Product1 = (((Experiment_LQR_P.K_LQR[0] * Experiment_LQR_B.Sum_b[0] +
Experiment_LQR_P.K_LQR[1] * Experiment_LQR_B.Sum_b[1]) +
Experiment_LQR_P.K_LQR[2] * Experiment_LQR_B.Sum_b[2]) +
Experiment_LQR_P.K_LQR[3] * Experiment_LQR_B.Sum_b[3]) *
(real_T)(fabs(Experiment_LQR_P.alpha_Gain[0] * rtb_WheelRaduis2) < 0.375);
/* Start for MATLABSystem: '<S6>/Digital Output' incorporates:
* Constant: '<S5>/Constant'
* DataTypeConversion: '<S6>/Data Type Conversion'
* MATLABSystem: '<S6>/Digital Output'
* RelationalOperator: '<S5>/Compare'
*/
writeDigitalPin((uint8_T)Experiment_LQR_PinNumber, (uint8_T)(rtb_Product1 <
Experiment_LQR_P.Constant_Value));
/* Saturate: '<S4>/Saturation -Vs to Vs' */
if (rtb_Product1 > Experiment_LQR_P.Vs) {
rtb_Product1 = Experiment_LQR_P.Vs;
} else {
if (rtb_Product1 < Experiment_LQR_P.SaturationVstoVs_LowerSat) {
rtb_Product1 = Experiment_LQR_P.SaturationVstoVs_LowerSat;
}
}
/* Gain: '<S4>/conversion to dutycycle (convert to uint8, overflow will provide reverse polairty magnitued)' incorporates:
* Saturate: '<S4>/Saturation -Vs to Vs'
*/
tmp = (real32_T)floor(Experiment_LQR_P.conversiontodutycycleconverttou *
(real32_T)rtb_Product1);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 256.0F);
}
Experiment_LQR_B.conversiontodutycycleconverttou = (uint8_T)(tmp < 0.0F ?
(int16_T)(uint8_T)-(int8_T)(uint8_T)-tmp : (int16_T)(uint8_T)tmp);
/* End of Gain: '<S4>/conversion to dutycycle (convert to uint8, overflow will provide reverse polairty magnitued)' */
/* S-Function (arduinoanalogoutput_sfcn): '<S7>/PWM' */
MW_analogWrite(Experiment_LQR_P.PWM_pinNumber,
Experiment_LQR_B.conversiontodutycycleconverttou);
/* Gain: '<Root>/X state' */
Experiment_LQR_B.Xstate[0] = Experiment_LQR_P.Xstate_Gain[0] *
rtb_WheelRaduis2;
Experiment_LQR_B.Xstate[1] = Experiment_LQR_P.Xstate_Gain[1] * rtb_Sum6;
Experiment_LQR_B.Xstate[2] = Experiment_LQR_P.Xstate_Gain[2] *
Experiment_LQR_B.WheelRaduis;
Experiment_LQR_B.Xstate[3] = Experiment_LQR_P.Xstate_Gain[3] *
rtb_convertoradianssec;
/* Update for Delay: '<S12>/Delay' */
Experiment_LQR_DW.Delay_DSTATE = Experiment_LQR_B.Sum3;
/* Update for UnitDelay: '<S10>/UD' */
Experiment_LQR_DW.UD_DSTATE = rtb_TSamp;
/* External mode */
rtExtModeUploadCheckTrigger(2);
{ /* Sample time: [0.0s, 0.0s] */
rtExtModeUpload(0, Experiment_LQR_M->Timing.t[0]);
}
{ /* Sample time: [0.01s, 0.0s] */
rtExtModeUpload(1, (((Experiment_LQR_M->Timing.clockTick1+
Experiment_LQR_M->Timing.clockTickH1* 4294967296.0)) *
0.01));
}
/* signal main to stop simulation */
{ /* Sample time: [0.0s, 0.0s] */
if ((rtmGetTFinal(Experiment_LQR_M)!=-1) &&
!((rtmGetTFinal(Experiment_LQR_M)-Experiment_LQR_M->Timing.t[0]) >
Experiment_LQR_M->Timing.t[0] * (DBL_EPSILON))) {
rtmSetErrorStatus(Experiment_LQR_M, "Simulation finished");
}
if (rtmGetStopRequested(Experiment_LQR_M)) {
rtmSetErrorStatus(Experiment_LQR_M, "Simulation finished");
}
}
/* Update absolute time for base rate */
/* The "clockTick0" counts the number of times the code of this task has
* been executed. The absolute time is the multiplication of "clockTick0"
* and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
* overflow during the application lifespan selected.
* Timer of this task consists of two 32 bit unsigned integers.
* The two integers represent the low bits Timing.clockTick0 and the high bits
* Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
*/
if (!(++Experiment_LQR_M->Timing.clockTick0)) {
++Experiment_LQR_M->Timing.clockTickH0;
}
Experiment_LQR_M->Timing.t[0] = Experiment_LQR_M->Timing.clockTick0 *
Experiment_LQR_M->Timing.stepSize0 + Experiment_LQR_M->Timing.clockTickH0 *
Experiment_LQR_M->Timing.stepSize0 * 4294967296.0;
{
/* Update absolute timer for sample time: [0.01s, 0.0s] */
/* The "clockTick1" counts the number of times the code of this task has
* been executed. The resolution of this integer timer is 0.01, which is the step size
* of the task. Size of "clockTick1" ensures timer will not overflow during the
* application lifespan selected.
* Timer of this task consists of two 32 bit unsigned integers.
* The two integers represent the low bits Timing.clockTick1 and the high bits
* Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
*/
Experiment_LQR_M->Timing.clockTick1++;
if (!Experiment_LQR_M->Timing.clockTick1) {
Experiment_LQR_M->Timing.clockTickH1++;
}
}
}
/* Model initialize function */
void Experiment_LQR_initialize(void)
{
/* Registration code */
/* initialize non-finites */
rt_InitInfAndNaN(sizeof(real_T));
/* initialize real-time model */
(void) memset((void *)Experiment_LQR_M, 0,
sizeof(RT_MODEL_Experiment_LQR_T));
{
/* Setup solver object */
rtsiSetSimTimeStepPtr(&Experiment_LQR_M->solverInfo,
&Experiment_LQR_M->Timing.simTimeStep);
rtsiSetTPtr(&Experiment_LQR_M->solverInfo, &rtmGetTPtr(Experiment_LQR_M));
rtsiSetStepSizePtr(&Experiment_LQR_M->solverInfo,
&Experiment_LQR_M->Timing.stepSize0);
rtsiSetErrorStatusPtr(&Experiment_LQR_M->solverInfo, (&rtmGetErrorStatus
(Experiment_LQR_M)));
rtsiSetRTModelPtr(&Experiment_LQR_M->solverInfo, Experiment_LQR_M);
}
rtsiSetSimTimeStep(&Experiment_LQR_M->solverInfo, MAJOR_TIME_STEP);
rtsiSetSolverName(&Experiment_LQR_M->solverInfo,"FixedStepDiscrete");
rtmSetTPtr(Experiment_LQR_M, &Experiment_LQR_M->Timing.tArray[0]);
rtmSetTFinal(Experiment_LQR_M, -1);
Experiment_LQR_M->Timing.stepSize0 = 0.01;
/* External mode info */
Experiment_LQR_M->Sizes.checksums[0] = (40318599U);
Experiment_LQR_M->Sizes.checksums[1] = (678392467U);
Experiment_LQR_M->Sizes.checksums[2] = (1578117941U);
Experiment_LQR_M->Sizes.checksums[3] = (1365769257U);
{
static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
static RTWExtModeInfo rt_ExtModeInfo;
static const sysRanDType *systemRan[5