SMC_Rpbatic.rar_3 dof control_robot_sliding mode_sliding mode co

% Main Program_1 Name: Simulation Adaptive Sliding Mode control (ASMC) and & Sliding Mode controller (SMC) clc , clear all , close all % 1- Simulation parameters t0 = 0; % Simulation 1st step time tf = 1; % Simulation length dt = .00001; % Simulation step size (seconds) i = 1; % 2- Controller parameters lamda1 = 1; lamda2 = 1; lamda3 = 0.1; lamda = [lamda1 0 0 0 lamda2 0 0 0 lamda3]; k1 = 20; k2 = 10; k3 = 100; K = [k1 0 0 0 k2 0 0 0 k3]; gama1 = 60; gama2 = 40; gama3 =250; gama = [gama1 0 0 0 gama2 0 0 0 gama3]; fi1 = 0.01; fi2 = 0.01; fi3 = 0.01; betahat1 = 0; betahat2 = 0; betahat3 = 0; betahat = [betahat1 0 0 0 betahat2 0 0 0 betahat3]; % 2- parameter first step size x1 = 0.3; x2 = 0.0000001; x3 = 0.0000001; X = [x1; x2; x3]; x1dot = 0.0000001; x2dot = 0.0000001; x3dot = 0.000001; xdot = [x1dot; x2dot;x3dot]; F = [0.0000001; 0.0000001; 0.0000001]; M = [0.0000001 0.0000001 0.0000001 0.0000001 0.0000001 0.0000001 0.0000001 0.0000001 0.0000001]; w1 = 0; w2 = 0; w3 = 0; k5 = 50; k6 = 35; k7 = 51; b5 = 5; b6 = 3.4; b7 = 4.6; for t=t0+dt:dt:tf % Desired inputs %hip angle ah1 =22.47; bh1 =5.467; ch1 =2.226; ah2 =5.492; bh2 =11.15; ch2 =-1.265; ah3 =7.951; bh3 =1.212; ch3 =1.939; ah4 =1.586; bh4 =19.16; ch4 =-0.2865; hip_angle = ah1*sin(bh1*t+ch1) + ah2*sin(bh2*t+ch2) + ah3*sin(bh3*t+ch3) + ah4*sin(bh4*t+ch4); hip_angle_d = ah1*bh1*cos(bh1*t+ch1) + ah2*bh2*cos(bh2*t+ch2) + ah3*bh3*cos(bh3*t+ch3) + ah4*bh4*cos(bh4*t+ch4); hip_angle_dd = -ah1*bh1*bh1*sin(bh1*t+ch1) - ah2*bh2*bh2*sin(bh2*t+ch2) - ah3*bh3*bh3*sin(bh3*t+ch3) + ah4*bh4*bh4*sin(bh4*t+ch4); %knee angle ak1 =119.9; bk1 =2.324; ck1 =-1.488; ak2 =14.75; bk2 =12.16; ck2 =2.142; ak3 =108.1; bk3 =3.446; ck3 =1.23; ak4 =3.773; bk4 =18.5; ck4 =3.165; knee_angle =ak1*sin(bk1*t+ck1) + ak2*sin(bk2*t+ck2) + ak3*sin(bk3*t+ck3) + ak4*sin(bk4*t+ck4); knee_angle_d =ak1*bk1*cos(bk1*t+ck1) + ak2*bk2*cos(bk2*t+ck2) + ak3*bk3*cos(bk3*t+ck3) + ak4*bk4*cos(bk4*t+ck4); knee_angle_dd = -ak1*bk1*bk1*sin(bk1*t+ck1) - ak2*bk2*bk2*sin(bk2*t+ck2) - ak3*bk3*bk3*sin(bk3*t+ck3) - ak4*bk4*bk4*sin(bk4*t+ck4); %ankle angle aL1 = 15; bL1 = 0.9133; cL1 = 2.271; aL2 = 8.752; bL2 = 11.12; cL2 = 3.413; aL3 = 3.969; bL3 = 20.06; cL3 = -2.352; aL4 = 1.219; bL4 = 29.21; cL4 = 4.468; ankle_angle = aL1*sin(bL1*t+cL1) + aL2*sin(bL2*t+cL2) + aL3*sin(bL3*t+cL3) + aL4*sin(bL4*t+cL4); ankle_angle_d = aL1*bL1*cos(bL1*t+cL1) + aL2*bL2*cos(bL2*t+cL2) + aL3*bL3*cos(bL3*t+cL3) + aL4*bL4*cos(bL4*t+cL4); ankle_angle_dd = -aL1*bL1*bL1*sin(bL1*t+cL1) - aL2*bL2*bL2*sin(bL2*t+cL2) - aL3*bL3*bL3*sin(bL3*t+cL3) - aL4*bL4*bL4*sin(bL4*t+cL4); x1d = hip_angle*0.0174533; x2d = knee_angle*0.0174533; x3d = ankle_angle*0.0174533; x1dotd = hip_angle_d*0.0174533; x2dotd = knee_angle_d*0.0174533; x3dotd = ankle_angle_d*0.0174533; x1ddotd = hip_angle_dd*0.0174533; x2ddotd = knee_angle_dd*0.0174533; x3ddotd = ankle_angle_dd*0.0174533; xd = [x1d; x2d; x3d]; xddotd = [x1dotd; x2dotd; x3dotd]; xdotd = [x1ddotd; x2ddotd; x3ddotd]; % Errors & sliding surface e1 = x1 - x1d; e2 = x2 - x2d; e3 = x3 - x3d; E = [e1; e2; e3]; e1dot = x1dot - x1dotd; e2dot = x2dot - x2dotd; e3dot = x3dot - x3dotd; s1 = e1dot + lamda1*e1; s2 = e2dot + lamda2*e2; s3 = e3dot + lamda3*e3; S = [s1; s2; s3]; SAS = [tanh(s1/fi1);tanh(s2/fi2);tanh(s3/fi3)]; % SAS = [sign(s1);sign(s2);sign(s3)]; %SMC controller******************************************* uhat = F + lamda * xdot - xddotd-lamda*xdotd; u = -M*(uhat+K*SAS); u1 = u(1); u2 = u(2); u3 = u(3); % Robot Model F11 = ((11763715046721856000*cos(x1 + x2 + x3) + 106226655360929292800*sin(x1 + x2) + (9007199254740992000*abs(x1dot)^2*((45664619*x1dot*cos(x2 + x3))/800000000 + (45664619*x2dot*cos(x2 + x3))/1600000000 + (45664619*x3dot*cos(x2 + x3))/1600000000 + (5534504347741717*x1dot*sin(x2))/10737418240000000 + (5534504347741717*x2dot*sin(x2))/21474836480000000))/x1dot - (1001*abs(x3dot)^2*(6420303481281708032*x1dot*cos(x2 + x3) + 29874065178208763904*x1dot*cos(x3) + 29874065178208763904*x2dot*cos(x3) + 14937032589104381952*x3dot*cos(x3)))/(25000*x3dot) - (7*x1dot*abs(x2dot)^2*(7344827182586273988608*cos(x2 + x3) + 66323982067858489212928*sin(x2)))/(200000*x2dot))*(2056551611124156928*sin(x2 + x3) - 512359998580948488342354366673453056*cos(x2) + 5040923979754750976*sin(x3) + 183141495249342363433016097918418944*sin(x3)^2 + 157437425740662740273673935893037056*sin(x2 + x3)*sin(x3) - 471817132194990779153919150236106752))/(17059968996504075891269839758204862464*cos(x2 - 2*x3) - 11180220120557258834093271241778528256*cos(x2 + 2*x3) - 21514141505820484359867503957302575104*sin(x2 - x3) - 11171999791667153019146744770846523392*sin(2*x2 + x3) + 1889249108887953104645016115231064064*sin(x2 + 3*x3) + 823895742478805979025803673636634698225834421055913984*cos(2*x2) - 589543411492813995511068673554029454952450571469062144*cos(2*x3) + 88190305301457403904*sin(3*x3) + 76153771590532872349748306835289841558655228033630208*cos(2*x2 + 2*x3) + 1624091339219468427310848395968512000*sin(2*x2 + 3*x3) + 2040120763818985010532898462228283392*sin(x2 + x3) - 13885373853774135093820403129664929792*cos(x2) + 92349458752009783778325507207816282112*sin(x3) - 2852455708190053405293869288127316643145127488837910528) - ((94109720373774848000*cos(x1 + x2 + x3) + (72057594037927936000*abs(x2dot)^2*((53120067*x1dot*cos(x3))/800000000 - (45664619*x1dot*cos(x2 + x3))/1600000000 + (53120067*x2dot*cos(x3))/800000000 + (53120067*x3dot*cos(x3))/1600000000))/x2dot - (1001*abs(x3dot)^2*(6420303481281708032*x1dot*cos(x2 + x3) + 7468516294552189952*x1dot*cos(x3) + 7468516294552189952*x2dot*cos(x3)))/(3125*x3dot) + (131157628260469178368*abs(x1dot)^2*(98*x1dot*cos(x2 + x3) + 49*x2dot*cos(x2 + x3) + 49*x3dot*cos(x2 + x3) + 114*x1dot*cos(x3) + 114*x2dot*cos(x3) + 57*x3dot*cos(x3)))/(3125*x1dot))*(177708010380250790534060830814633984*cos(x2)*sin(x3) - 64044999822618561042794295834181632*cos(x2) - 7496023691629522944*sin(x3) - 140678173064908362136097227785895936*sin(x2 + x3) + 1837298027113695744*sin(x3)^2 + 19679678217582844840052251200323584*sin(x2 + x3)*sin(x3) + 2245424774336517888))/(17059968996504075891269839758204862464*cos(x2 - 2*x3) - 11180220120557258834093271241778528256*cos(x2 + 2*x3) - 21514141505820484359867503957302575104*sin(x2 - x3) - 11171999791667153019146744770846523392*sin(2*x2 + x3) + 1889249108887953104645016115231064064*sin(x2 + 3*x3) + 823895742478805979025803673636634698225834421055913984*cos(2*x2) - 589543411492813995511068673554029454952450571469062144*cos(2*x3) + 88190305301457403904*sin(3*x3) + 76153771590532872349748306835289841558655228033630208*cos(2*x2 + 2*x3) + 1624091339219468427310848395968512000*sin(2*x2 + 3*x3) + 2040120763818985010532898462228283392*sin(x2 + x3) - 13885373853774135093820403129664929792*cos(x2) + 92349458752009783778325507207816282112*sin(x3) - 2852455708190053405293869288127316643145127488837910528) - ((90809213427305970230350942142977802240*sin(x3) + 1649591939522027268684972788108014085729202065222139904*sin(x3)^2 - 4249750921480753102120609760006430435602798633591767040)*((183808047605029*cos(x1 + x2 + x3))/140737488355328 + (82