├── README.md └── src ├── TargetAligned ├── sunposition.m ├── date.m ├── alpha_azimuthcalc.m └── TA_NEW.m ├── AzimuthElevation ├── sunposition.m ├── Rot_mat_exp.m ├── for_mainprog_AZ_EL_3_UPU.m ├── date.m ├── alpha_azimuthcalc.m ├── Rotmatrix_AzEl.m ├── TEST.m └── mainprog_AZ_EL_3_UPU.m ├── 3-rps ├── sunposition.m ├── top_platform.m ├── base_platform.m ├── for_mainprog_3RPS.m ├── date.m ├── alpha_azimuthcalc.m └── final_program1.m ├── expt ├── 3-rps │ ├── sunposition.m │ ├── MOTORSTOP.m │ ├── RunMotor3.m │ ├── RunMotor2.m │ ├── RunMotor1.m │ ├── for_mainprog_3RPS.m │ ├── date.m │ ├── Homing.m │ ├── alpha_azimuthcalc.m │ ├── Encoder_calibration.m │ ├── TEST.m │ ├── ICER_3RPS_Expt.m │ └── RPS_lab_FINAL.m ├── az-el │ ├── sunposition.m │ ├── date.m │ ├── alpha_azimuthcalc.m │ ├── Track_point.m │ ├── ICER_expt.m │ └── Az_El.m └── error │ ├── sunposition.m │ ├── Refl_ray.m │ ├── top_platform.m │ ├── base_platform.m │ ├── for_mainprog_3RPS_test.m │ ├── confuneq_error_min.m │ ├── for_mainprog_3RPS.m │ ├── date.m │ ├── alpha_azimuthcalc.m │ ├── error_min.m │ ├── areaintersection.m │ ├── TEST_Error.m │ └── ERROR.m └── paper └── A heliostat based on three-dof parallel manipulator.pdf /README.md: -------------------------------------------------------------------------------- 1 | # 3-RPS-parallel_robot_heliostat 2 | Modeling, simulation and control of Azimuth-Elevation, Target-Aligned and the proposed 3-RPS parallel manipulator 3 | -------------------------------------------------------------------------------- /src/TargetAligned/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x, y ,z]=sunposition(alpha,A) 2 | x=cosd(alpha).*sind(A); 3 | y=cosd(alpha).*cosd(A); 4 | z=sind(alpha); 5 | 6 | -------------------------------------------------------------------------------- /src/AzimuthElevation/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x, y, z]=sunposition(alpha,A) 2 | x=cosd(alpha).*sind(A); 3 | y=cosd(alpha).*cosd(A); 4 | z=sind(alpha); 5 | 6 | -------------------------------------------------------------------------------- /src/3-rps/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x ,y, z]=sunposition(elevation,azimuth) 2 | x=cosd(elevation).*sind(azimuth); 3 | y=cosd(elevation).*cosd(azimuth); 4 | z=sind(elevation); 5 | 6 | -------------------------------------------------------------------------------- /src/expt/3-rps/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x ,y, z]=sunposition(elevation,azimuth) 2 | x=cosd(elevation).*sind(azimuth); 3 | y=cosd(elevation).*cosd(azimuth); 4 | z=sind(elevation); 5 | 6 | -------------------------------------------------------------------------------- /src/expt/az-el/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x ,y, z]=sunposition(elevation,azimuth) 2 | x=cosd(elevation).*sind(azimuth); 3 | y=cosd(elevation).*cosd(azimuth); 4 | z=sind(elevation); 5 | 6 | -------------------------------------------------------------------------------- /src/expt/error/sunposition.m: -------------------------------------------------------------------------------- 1 | function [x ,y, z]=sunposition(elevation,azimuth) 2 | x=cosd(elevation).*sind(azimuth); 3 | y=cosd(elevation).*cosd(azimuth); 4 | z=sind(elevation); 5 | 6 | -------------------------------------------------------------------------------- /src/paper/A heliostat based on three-dof parallel manipulator.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/basharbme/3-RPS-parallel_robot_heliostat/master/src/paper/A heliostat based on three-dof parallel manipulator.pdf -------------------------------------------------------------------------------- /src/AzimuthElevation/Rot_mat_exp.m: -------------------------------------------------------------------------------- 1 | axis = [0;0;1]; 2 | Skew_axis_matrix = [0 -axis(3) axis(2); 3 | axis(3) 0 -axis(1); 4 | -axis(2) axis(1) 0]; 5 | Rz = expm(180*Skew_axis_matrix) -------------------------------------------------------------------------------- /src/expt/error/Refl_ray.m: -------------------------------------------------------------------------------- 1 | function F=Refl_ray(r,OS,N) 2 | 3 | 4 | r1=r(1);r2=r(2);r3=r(3); 5 | 6 | 7 | F=[ r1^2 + r2^2 + r3^2 - 1; 8 | cross(OS,N)- cross(N,r); 9 | dot(OS,N)- dot(N,r); 10 | ]; 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/3-rps/top_platform.m: -------------------------------------------------------------------------------- 1 | function P0p=top_platform(beta,rp) 2 | P0p=zeros(4,1,3); 3 | P0p(1,1,1)=rp; P0p(2,1,1)=0; P0p(4,1,1)=1; 4 | P0p(1,1,2)=rp*cosd(beta); P0p(2,1,2)=rp*sind(beta); P0p(4,1,2)=1; 5 | P0p(1,1,3)=rp*cosd(2*beta); P0p(2,1,3)=rp*sind(2*beta); P0p(4,1,3)=1; 6 | -------------------------------------------------------------------------------- /src/3-rps/base_platform.m: -------------------------------------------------------------------------------- 1 | function B0b=base_platform(alpha,rb) 2 | B0b=zeros(4,1,3); 3 | B0b(1,1,1)=rb; B0b(2,1,1)=0; B0b(4,1,1)=1; 4 | B0b(1,1,2)=rb*cosd(alpha); B0b(2,1,2)=rb*sind(alpha); B0b(4,1,2)=1; 5 | B0b(1,1,3)=rb*cosd(2*alpha); B0b(2,1,3)=rb*sind(2*alpha); B0b(4,1,3)=1; 6 | -------------------------------------------------------------------------------- /src/expt/error/top_platform.m: -------------------------------------------------------------------------------- 1 | function P0p=top_platform(beta,rp) 2 | P0p=zeros(4,1,3); 3 | P0p(1,1,1)=rp; P0p(2,1,1)=0; P0p(4,1,1)=1; 4 | P0p(1,1,2)=rp*cosd(beta); P0p(2,1,2)=rp*sind(beta); P0p(4,1,2)=1; 5 | P0p(1,1,3)=rp*cosd(2*beta); P0p(2,1,3)=rp*sind(2*beta); P0p(4,1,3)=1; 6 | -------------------------------------------------------------------------------- /src/expt/error/base_platform.m: -------------------------------------------------------------------------------- 1 | function B0b=base_platform(alpha,rb) 2 | B0b=zeros(4,1,3); 3 | B0b(1,1,1)=rb; B0b(2,1,1)=0; B0b(4,1,1)=1; 4 | B0b(1,1,2)=rb*cosd(alpha); B0b(2,1,2)=rb*sind(alpha); B0b(4,1,2)=1; 5 | B0b(1,1,3)=rb*cosd(2*alpha); B0b(2,1,3)=rb*sind(2*alpha); B0b(4,1,3)=1; 6 | -------------------------------------------------------------------------------- /src/expt/error/for_mainprog_3RPS_test.m: -------------------------------------------------------------------------------- 1 | function F=for_mainprog_3RPS_test(x,A,O1G) 2 | global rp 3 | 4 | n1=x(1);n3=x(2);o3=x(3); xc=O1G(1);yc=O1G(2); 5 | 6 | n2=-yc/rp; 7 | o1=n2; 8 | o2=n1-(2*xc/rp); 9 | 10 | F=[ n1^2 + n2^2 + n3^2 - 1; 11 | o1^2 + o2^2 + o3^2 - 1; 12 | A(1)*n1 + A(2)*n2 + A(3)*n3; 13 | % n1*o1 + n2*o2 + n3*o3; 14 | % A(1)*o1 + A(2)*o2 + A(3)*o3 15 | ]; 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/expt/3-rps/MOTORSTOP.m: -------------------------------------------------------------------------------- 1 | function MOTORSTOP 2 | global ard 3 | % global fileID 4 | % fileID = fopen('EXP.txt','w'); 5 | % disp('ALL MOTORS STOPPED') 6 | % fprintf(fileID,'ALL MOTORS STOPPED\n'); 7 | 8 | % to stop leg 1 9 | ard.digitalWrite(30,0); 10 | ard.analogWrite(4,0); 11 | ard.digitalWrite(31,0); 12 | ard.analogWrite(5,0); 13 | 14 | % to stop leg 3 15 | ard.analogWrite(8,0); 16 | ard.analogWrite(9,0); 17 | ard.digitalWrite(34,0); 18 | ard.digitalWrite(35,0); 19 | % 20 | % to stop leg2 21 | ard.analogWrite(6,0); 22 | ard.analogWrite(7,0); 23 | ard.digitalWrite(32,0); 24 | ard.digitalWrite(33,0); -------------------------------------------------------------------------------- /src/expt/3-rps/RunMotor3.m: -------------------------------------------------------------------------------- 1 | function RunMotor3(act_time) 2 | global ard 3 | % global fileID 4 | % fileID = fopen('EXP.txt','w'); 5 | if act_time <0 6 | % disp('Motor 3 back') 7 | % fprintf(fileID,'Motor 3 back\n'); 8 | % bwd 9 | ard.digitalWrite(34,0); 10 | ard.analogWrite(8,0); 11 | ard.digitalWrite(35,1); 12 | ard.analogWrite(9,200); 13 | else 14 | % disp('Motor 3 fwd') 15 | % fprintf(fileID,'Motor 3 fwd\n'); 16 | % fwd 17 | ard.digitalWrite(35,0); 18 | ard.analogWrite(9,0); 19 | ard.digitalWrite(34,1); 20 | ard.analogWrite(8,200); 21 | end -------------------------------------------------------------------------------- /src/AzimuthElevation/for_mainprog_AZ_EL_3_UPU.m: -------------------------------------------------------------------------------- 1 | function F=for_mainprog_AZ_EL_3_UPU(x,OS) 2 | global OP OO1 O1M Rot_gam 3 | 4 | n1=x(1);n2=x(2);n3=x(3);o1=x(4);o2=x(5); 5 | o3=0; 6 | O1P_bcs=Rot_gam'*(OP-OO1); % O1P_bcs = O1P described in bcs 7 | g=O1P_bcs-O1M; 8 | GP=g/norm(g,2); % unit reflected ray wrt bcs 9 | GS=Rot_gam'*OS; % Sun vector in bcs 10 | GN=(GS+GP)/norm((GS+GP),2); % unit normal ray in bcs 11 | a1=GN(1); a2=GN(2); a3=GN(3); 12 | 13 | F=[ n1^2 + n2^2 + n3^2 - 1; 14 | o1^2 + o2^2 + o3^2 - 1; 15 | a1*n1 + a2*n2 + a3*n3; 16 | n1*o1 + n2*o2 + n3*o3; 17 | a1*o1 + a2*o2 + a3*o3 18 | ]; 19 | 20 | -------------------------------------------------------------------------------- /src/expt/3-rps/RunMotor2.m: -------------------------------------------------------------------------------- 1 | function RunMotor2(act_time) 2 | global ard 3 | % global fileID 4 | % fileID = fopen('EXP.txt','w'); 5 | if act_time <0 6 | % disp('Motor 2 back') 7 | % fprintf(fileID,'Motor 2 back\n'); 8 | % bwd 9 | ard.digitalWrite(32,0); 10 | ard.analogWrite(6,0); 11 | ard.digitalWrite(33,1); 12 | ard.analogWrite(7,200); 13 | else 14 | % disp('Motor 2 fwd') 15 | % fprintf(fileID,'Motor 2 fwd \n'); 16 | % fwd 17 | ard.digitalWrite(33,0); 18 | ard.analogWrite(7,0); 19 | ard.digitalWrite(32,1); 20 | ard.analogWrite(6,200); 21 | end 22 | -------------------------------------------------------------------------------- /src/expt/3-rps/RunMotor1.m: -------------------------------------------------------------------------------- 1 | function RunMotor1(act_time) 2 | global ard 3 | % fileID = fopen('EXP.txt','w'); 4 | % global fileID 5 | if act_time <0 6 | % bck 7 | % disp('Motor 1 back') 8 | % fprintf(fileID,'Motor 1 back\n'); 9 | ard.digitalWrite(30,0); 10 | ard.analogWrite(4,0); 11 | ard.digitalWrite(31,1); 12 | ard.analogWrite(5,200); 13 | else 14 | % fwd 15 | % disp('Motor 1 fwd') 16 | % fprintf(fileID,'Motor 1 fwd\n'); 17 | ard.digitalWrite(31,0); 18 | ard.analogWrite(5,0); 19 | ard.digitalWrite(30,1); 20 | ard.analogWrite(4,200); 21 | end 22 | 23 | -------------------------------------------------------------------------------- /src/expt/3-rps/for_mainprog_3RPS.m: -------------------------------------------------------------------------------- 1 | function F=for_mainprog_3RPS(x,OS) 2 | global OP rp zc t Rot 3 | 4 | n1=x(1);n3=x(2);o3=x(3);xc=x(4);yc=x(5); 5 | 6 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 7 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 8 | g=O1P_bcs-O1G; 9 | GP=g/norm(g,2); 10 | GS=Rot'*OS; % Sun vector in bcs 11 | A=(GS+GP)/norm((GS+GP),2); 12 | % a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 13 | n2=-yc/rp; 14 | o1=n2; 15 | o2=n1-(2*xc/rp); 16 | 17 | F=[ n1^2 + n2^2 + n3^2 - 1; 18 | o1^2 + o2^2 + o3^2 - 1; 19 | A(1)*n1 + A(2)*n2 + A(3)*n3; 20 | n1*o1 + n2*o2 + n3*o3; 21 | A(1)*o1 + A(2)*o2 + A(3)*o3 22 | ]; 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/expt/error/confuneq_error_min.m: -------------------------------------------------------------------------------- 1 | function [c,ceq] = confuneq_error_min(X) 2 | global rp 3 | n1 = X(1); n3=X(2); o3 = X(3); a1N=X(4); a2N = X(5); a3N = X(6); 4 | xc = X(7); yc = X(8); 5 | n2 = -yc/rp; 6 | o1 = n2; 7 | o2 = n1 - (2*xc/rp); 8 | % Nonlinear inequality constraints 9 | c = []; 10 | % Nonlinear equality constraints 11 | % ceq = [n1^2 + n2^2 + n3^2 -1; 12 | % o1^2 + o2^2 + o3^2 -1; 13 | % a1N^2 + a2N^2 + a3N^2-1; 14 | % n1*o1 + n2*o2 + n3*o3; 15 | % a1N*n1 + a2N*n2 + a3N*n3; 16 | % a1N*o1 + a2N*o2 + a3N*o3]; 17 | 18 | ceq(1) = n1^2 + n2^2 + n3^2 -1; 19 | ceq(2) = o1^2 + o2^2 + o3^2 -1; 20 | ceq(3) = a1N^2 + a2N^2 + a3N^2-1; 21 | ceq(4) = n1*o1 + n2*o2 + n3*o3; 22 | ceq(5) = a1N*n1 + a2N*n2 + a3N*n3; 23 | ceq(6) = a1N*o1 + a2N*o2 + a3N*o3; 24 | % ceq(7) = n2 + yc/rp; 25 | % ceq(8) = o1- n2; 26 | % ceq(9) = o2 - n1+ (2*xc/rp); 27 | -------------------------------------------------------------------------------- /src/3-rps/for_mainprog_3RPS.m: -------------------------------------------------------------------------------- 1 | function F=for_mainprog_3RPS(x,OS) 2 | global OP rp zc t Rot 3 | 4 | n1=x(1);n3=x(2);o3=x(3);xc=x(4);yc=x(5); 5 | 6 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 7 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 8 | g=O1P_bcs-O1G; 9 | GP=g/norm(g,2); 10 | GS=Rot'*OS; % Sun vector in bcs 11 | A=(GS+GP)/norm((GS+GP),2); 12 | % a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 13 | n2=-yc/rp; 14 | o1=n2; 15 | o2=n1-(2*xc/rp); 16 | 17 | F=[ n1^2 + n2^2 + n3^2 - 1; 18 | o1^2 + o2^2 + o3^2 - 1; 19 | A(1)*n1 + A(2)*n2 + A(3)*n3; 20 | n1*o1 + n2*o2 + n3*o3; 21 | A(1)*o1 + A(2)*o2 + A(3)*o3 22 | ]; 23 | % F=[ n1^2 + (-yc/rp)^2 + n3^2 - 1; 24 | % (-yc/rp)^2 + (n1-2*xc/rp)^2 + o3^2 - 1; 25 | % A(1)*n1 + A(2)*(-yc/rp) + A(3)*n3; 26 | % 2*n1*(-yc/rp) - 2*(-yc/rp)*(xc/rp) + n3*o3; 27 | % A(1)*(-yc/rp) + A(2)*(n1-2*xc/rp) + A(3)*o3; 28 | % ]; 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/expt/error/for_mainprog_3RPS.m: -------------------------------------------------------------------------------- 1 | function F=for_mainprog_3RPS(x,OS) 2 | global OP rp zc t Rot 3 | 4 | n1=x(1);n3=x(2);o3=x(3);xc=x(4);yc=x(5); 5 | 6 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 7 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 8 | g=O1P_bcs-O1G; 9 | GP=g/norm(g,2); 10 | GS=Rot'*OS; % Sun vector in bcs 11 | A=(GS+GP)/norm((GS+GP),2); 12 | % a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 13 | n2=-yc/rp; 14 | o1=n2; 15 | o2=n1-(2*xc/rp); 16 | 17 | F=[ n1^2 + n2^2 + n3^2 - 1; 18 | o1^2 + o2^2 + o3^2 - 1; 19 | A(1)*n1 + A(2)*n2 + A(3)*n3; 20 | n1*o1 + n2*o2 + n3*o3; 21 | A(1)*o1 + A(2)*o2 + A(3)*o3 22 | ]; 23 | % F=[ n1^2 + (-yc/rp)^2 + n3^2 - 1; 24 | % (-yc/rp)^2 + (n1-2*xc/rp)^2 + o3^2 - 1; 25 | % A(1)*n1 + A(2)*(-yc/rp) + A(3)*n3; 26 | % 2*n1*(-yc/rp) - 2*(-yc/rp)*(xc/rp) + n3*o3; 27 | % A(1)*(-yc/rp) + A(2)*(n1-2*xc/rp) + A(3)*o3; 28 | % ]; 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/3-rps/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/expt/3-rps/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/expt/az-el/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/expt/error/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/TargetAligned/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/AzimuthElevation/date.m: -------------------------------------------------------------------------------- 1 | function d=date(year,month,day) 2 | if(rem(year,4)==0) 3 | if (month==1) 4 | d=day; 5 | elseif(month==2) 6 | d=day+31; 7 | elseif(month==3) 8 | d=day+60; 9 | elseif(month==4) 10 | d=day+91; 11 | elseif(month==5) 12 | d=day+121; 13 | elseif(month==6) 14 | d=day+152; 15 | elseif(month==7) 16 | d=day+182; 17 | elseif(month==8) 18 | d=day+213; 19 | elseif(month==9) 20 | d=day+244; 21 | elseif(month==10) 22 | d=day+274; 23 | elseif(month==11) 24 | d=day+305; 25 | elseif(month==12) 26 | d=day+335; 27 | end 28 | end 29 | 30 | 31 | if (rem(year,4)~=0) 32 | if (month==1) 33 | d=day; 34 | elseif(month==2) 35 | d=day+31; 36 | elseif(month==3) 37 | d=day+59; 38 | elseif(month==4) 39 | d=day+90; 40 | elseif(month==5) 41 | d=day+120; 42 | elseif(month==6) 43 | d=day+151; 44 | elseif(month==7) 45 | d=day+181; 46 | elseif(month==8) 47 | d=day+212; 48 | elseif(month==9) 49 | d=day+243; 50 | elseif(month==10) 51 | d=day+273; 52 | elseif(month==11) 53 | d=day+304; 54 | elseif(month==12) 55 | d=day+334; 56 | end 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/expt/3-rps/Homing.m: -------------------------------------------------------------------------------- 1 | % delete(instrfind({'PORT'},{'COM7'})) 2 | % a = arduino('COM7'); 3 | Act_tme = [0 -1 1]; 4 | tic 5 | while(toc <= 20) 6 | %% Leg1 7 | if(Act_tme(1) <0) 8 | % bck 9 | ard.digitalWrite(30,0); 10 | ard.analogWrite(4,0); 11 | ard.digitalWrite(31,1); 12 | ard.analogWrite(5,200); 13 | % 14 | else 15 | % fwd 16 | ard.digitalWrite(31,0); 17 | ard.analogWrite(5,0); 18 | ard.digitalWrite(30,1); 19 | ard.analogWrite(4,200); 20 | end 21 | %% Leg 3 22 | if(Act_tme(3) <0) 23 | % bwd 24 | ard.digitalWrite(34,0); 25 | ard.analogWrite(8,0); 26 | ard.digitalWrite(35,1); 27 | ard.analogWrite(9,200); 28 | else 29 | % % fwd 30 | ard.digitalWrite(35,0); 31 | ard.analogWrite(9,0); 32 | ard.digitalWrite(34,1); 33 | ard.analogWrite(8,200); 34 | 35 | end 36 | if(Act_tme(2) <0) 37 | %% Leg 2 38 | 39 | % bwd 40 | ard.digitalWrite(32,0); 41 | ard.analogWrite(6,0); 42 | ard.digitalWrite(33,1); 43 | ard.analogWrite(7,200); 44 | else 45 | % % fwd 46 | ard.digitalWrite(33,0); 47 | ard.analogWrite(7,0); 48 | ard.digitalWrite(32,1); 49 | ard.analogWrite(6,200); 50 | 51 | end 52 | end 53 | % to stop leg 1 54 | ard.digitalWrite(30,0); 55 | ard.analogWrite(4,0); 56 | ard.digitalWrite(31,0); 57 | ard.analogWrite(5,0); 58 | 59 | % to stop leg 3 60 | ard.analogWrite(8,0); 61 | ard.analogWrite(9,0); 62 | ard.digitalWrite(34,0); 63 | ard.digitalWrite(35,0); 64 | 65 | % to stop leg2 66 | ard.analogWrite(6,0); 67 | ard.analogWrite(7,0); 68 | ard.digitalWrite(32,0); 69 | ard.digitalWrite(33,0); -------------------------------------------------------------------------------- /src/expt/error/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth,a,HRA]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | a1=sind(delta)*cosd(latitude); 17 | a2=cosd(delta)*sind(latitude); 18 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 19 | % for i=1:length(HRA) 20 | % if (HRA(i)<=0) 21 | % azimuth(i)=a(i); 22 | % elseif(HRA(i)>0) 23 | % azimuth(i) =360-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 24 | % end 25 | % end 26 | 27 | for i=1:length(HRA) 28 | if (HRA(i)<=0 ) 29 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 30 | azimuth(i)=a(i); 31 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 32 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 33 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 34 | azimuth(i) =360-a(i); 35 | 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /src/TargetAligned/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | a1=sind(delta)*cosd(latitude); 17 | a2=cosd(delta)*sind(latitude); 18 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 19 | % for i=1:length(HRA) 20 | % if (HRA(i)<=0) 21 | % azimuth(i)=a(i); 22 | % elseif(HRA(i)>0) 23 | % azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 24 | % end 25 | % end 26 | 27 | for i=1:length(HRA) 28 | if (HRA(i)<=0 ) 29 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 30 | azimuth(i)=a(i); 31 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 32 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 33 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 34 | azimuth(i) =360-a(i); 35 | 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /src/expt/az-el/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | a1=sind(delta)*cosd(latitude); 17 | a2=cosd(delta)*sind(latitude); 18 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 19 | % for i=1:length(HRA) 20 | % if (HRA(i)<=0) 21 | % azimuth(i)=a(i); 22 | % elseif(HRA(i)>0) 23 | % azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 24 | % end 25 | % end 26 | 27 | for i=1:length(HRA) 28 | if (HRA(i)<=0 ) 29 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 30 | azimuth(i)=a(i); 31 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 32 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 33 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 34 | azimuth(i) =360-a(i); 35 | 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /src/AzimuthElevation/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | a1=sind(delta)*cosd(latitude); 17 | a2=cosd(delta)*sind(latitude); 18 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 19 | % for i=1:length(HRA) 20 | % if (HRA(i)<=0) 21 | % azimuth(i)=a(i); 22 | % elseif(HRA(i)>0) 23 | % azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 24 | % end 25 | % end 26 | 27 | for i=1:length(HRA) 28 | if (HRA(i)<=0 ) 29 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 30 | azimuth(i)=a(i); 31 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 32 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 33 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 34 | azimuth(i) =360-a(i); 35 | 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /src/3-rps/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | a1=sind(delta)*cosd(latitude); 17 | a2=cosd(delta)*sind(latitude); 18 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 19 | for i=1:length(HRA) 20 | if (HRA(i)<=0) 21 | azimuth(i)=a(i); 22 | elseif(HRA(i)>0) 23 | azimuth(i) =360-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 24 | end 25 | end 26 | 27 | for i=1:length(HRA) 28 | if (HRA(i)<=0 ) 29 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 30 | azimuth(i)=a(i); 31 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 32 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 33 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 34 | azimuth(i) =360-a(i); 35 | 36 | end 37 | end 38 | end 39 | 40 | % for i=1:length(HRA) 41 | % if (sind(HRA(i))>=0) 42 | % azimuth(i)=360-abs(a(i)); 43 | % else 44 | % azimuth(i) =a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 45 | % end 46 | % end 47 | -------------------------------------------------------------------------------- /src/AzimuthElevation/Rotmatrix_AzEl.m: -------------------------------------------------------------------------------- 1 | function [rotmat,rotmatZ,eta,Zeta]=Rotmatrix_AzEl(alpha,A,ang) 2 | % a=input('enter the x-cordinate of the receiver\n'); 3 | % b=input('enter the y-cordinate of the receiver\n'); 4 | % c=input('enter the z-cordinate of the receiver\n'); 5 | % a=-60;b=-60;c=120; 6 | % a=0;b=-6;c=12; 7 | rotmat=zeros(3,3,length(alpha)); 8 | [xs, ys, zs]=sunposition(alpha',A'); 9 | global OP O1M OO1 Rot_gam 10 | OM=Rot_gam*O1M+OO1; % wrt gcs 11 | for i=1:length(A) 12 | OS =[xs(1,i);ys(1,i);zs(1,i)]; % wrt gcs 13 | MS=OS; % wrt gcs ; unit sunray 14 | MP=(OP-OM)/norm((OP-OM),2); % wrt gcs ; unit reflected ray 15 | n = MS + MP; 16 | normal(:,:,i)=n/norm(n,2); % wrt gcs ; unit normal 17 | xn(i)=normal(1,1,i); yn(i)=normal(2,1,i); zn(i)=normal(3,1,i); qq(i)=sqrt(xn(i)^2+yn(i)^2); 18 | eta(i)=atan2d(yn(i),xn(i)); % azimuth 19 | Zeta(i)=asind(zn(i)/sqrt(xn(i)^2+yn(i)^2+zn(i)^2)); % elevation 20 | R1 = [cosd(eta(i)) -sind(eta(i)) 0; 21 | sind(eta(i)) cosd(eta(i)) 0; 22 | 0 0 1]; 23 | R2 = [cosd(90-Zeta(i)) 0 sind(90-Zeta(i)); 24 | 0 1 0 ; 25 | -sind(90-Zeta(i)) 0 cosd(90-Zeta(i))]; 26 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 27 | axis = [xn(i);yn(i);zn(i)]; 28 | Skew_axis_matrix = [ 0 -axis(3) axis(2); 29 | axis(3) 0 -axis(1); 30 | -axis(2) axis(1) 0]; 31 | Rz = expm((ang*pi/180)*Skew_axis_matrix); 32 | Ak = R1*R2; 33 | rotmatZ(:,:,i) = Rz*Ak; 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | 36 | rotmat(:,:,i) = R1*R2; 37 | % rotmat(:,:,i)=[cosd(eta(i))*sind(Zeta(i)) -sind(eta(i)) cosd(eta(i))*cosd(Zeta(i));sind(eta(i))*sind(Zeta(i)) cosd(eta(i)) sind(eta(i))*cosd(Zeta(i));-cosd(Zeta(i)) 0 sind(Zeta(i))]; 38 | % rotmat(:,:,i)=[normal2(1,1,i)*normal2(3,1,i) -normal2(2,1,i) normal2(1,1,i) ;normal2(2,1,i)*normal2(3,1,i) normal2(1,1,i) normal2(2,1,i) ;-(normal2(1,1,i)^2+normal2(2,1,i)^2) 0 normal2(3,1,i)]; 39 | % rotmat(:,:,i)=[xn(i)*zn(i)/qq(i) -yn(i)/qq(i) xn(i) ;yn(i)*zn(i)/qq(i) xn(i)/qq(i) yn(i);-(xn(i)^2+yn(i)^2)/qq(i) 0 zn(i)]; 40 | end 41 | 42 | -------------------------------------------------------------------------------- /src/expt/error/error_min.m: -------------------------------------------------------------------------------- 1 | function distN = error_min(X,a,b,c,OS,zeta) 2 | global rp t Rot m zc 3 | n1N = X(1); n3N = X(2); o3N = X(3); 4 | a1N = X(4); a2N = X(5); a3N = X(6); % wrt base coordinate system 5 | xcN = X(7); ycN = X(8); 6 | n2N = -ycN/rp; 7 | o1N = n2N; 8 | o2N = n1N - (2*xcN/rp); 9 | O1GN = [xcN;ycN;zc]; % new G in bcs 10 | OGN = t+Rot*O1GN; % This is the new point G in gcs 11 | RN = [n1N o1N a1N ; % Error corrected rotation matrix which 12 | n2N o2N a2N ; % takes mirror to base coordinate system 13 | n3N o3N a3N ]; 14 | 15 | Gm1= [m;m;0]; Gm2= [-m;m;0]; Gm3= [-m;-m;0]; Gm4= [m;-m;0]; % G is the centre of the mirror. 16 | Om1N = OGN + RN*Gm1; Om2N = OGN + RN*Gm2; %% mirror corners in gcs 17 | Om3N = OGN + RN*Gm3; Om4N = OGN + RN*Gm4; 18 | 19 | %% New reflected ray 20 | axy = cross(Rot'*OS,[a1N;a2N;a3N]); % IN bcs 21 | axs = axy/norm(axy,2); % IN bcs 22 | angle1 = acos(dot(Rot'*OS,[a1N;a2N;a3N])); % IN bcs); %%% RADIANS 23 | k1=axs(1); k2 = axs(2); k3 = axs(3); 24 | 25 | r11 = k1^2*(1-cos(angle1)) + cos(angle1); 26 | r12 = k1*k2*(1-cos(angle1))- k3*sin(angle1); 27 | r13 = k3*k1*(1-cos(angle1))+ k2*sin(angle1); 28 | r21 = k1*k2*(1-cos(angle1))+ k3*sin(angle1); 29 | r22 = k2^2*(1-cos(angle1)) + cos(angle1); 30 | r23 = k3*k2*(1-cos(angle1))- k1*sin(angle1); 31 | r31 = k3*k1*(1-cos(angle1))- k2*sin(angle1); 32 | r32 = k3*k2*(1-cos(angle1))+ k1*sin(angle1); 33 | r33 = k3^2*(1-cos(angle1)) + cos(angle1); 34 | Rot_ref = [r11 r12 r13; 35 | r21 r22 r23; 36 | r31 r32 r33]; 37 | GPN = Rot_ref*[a1N;a2N;a3N] ; 38 | 39 | % fhandle=@(r)Refl_ray(r,Rot'*OS,[a1N;a2N;a3N]); 40 | % [GPN,fvald] = fsolve(fhandle,r0); 41 | % % r0=GPN; 42 | % %%%% 43 | %% 44 | 45 | k1N=(cosd(zeta)*(a-Om1N(1)) + sind(zeta)* (b-Om1N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 46 | k2N=(cosd(zeta)*(a-Om2N(1)) + sind(zeta)* (b-Om2N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 47 | k3N=(cosd(zeta)*(a-Om3N(1)) + sind(zeta)* (b-Om3N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 48 | k4N=(cosd(zeta)*(a-Om4N(1)) + sind(zeta)* (b-Om4N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 49 | 50 | % hitting points on the receiver 51 | hp1N=Om1N+k1N*GPN; % in gcs 52 | hp2N=Om2N+k2N*GPN; % in gcs 53 | hp3N=Om3N+k3N*GPN; % in gcs 54 | hp4N=Om4N+k4N*GPN; % in gcs 55 | centroidN = (hp1N+hp2N+hp3N+hp4N)/4; 56 | distN = norm((centroidN-[a;b;c]),2); 57 | end -------------------------------------------------------------------------------- /src/expt/3-rps/alpha_azimuthcalc.m: -------------------------------------------------------------------------------- 1 | function [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude) 2 | % the input to this function is 3 | % LT=local time in seconds, 4 | % d is the number of days since the start of the year 5 | % longitude of the place in degrees, East is positive and west is negative 6 | % latitude of the place in degrees, North is positive and south is negative 7 | LSTM = 15*5.5 ; % (in degrees) local standard time meridian. 5.5 is the time differance between local time for india and GMT 8 | B=360*(d-81)/365; % in degrees 9 | %The equation of time (EoT) (in minutes) is an empirical equation that corrects for the eccentricity of the Earth's orbit and the Earth's axial tilt. 10 | EOT=9.87*sind(2*B)-7.53*cosd(B)-1.5*sind(B); % in minutes 11 | TC=4*(longitude-LSTM)+EOT; % Time Correction Factor (TC) in minutes 12 | LST=LT./3600+TC/60; % in hours 13 | HRA=15*(LST-12); % hour angle in degrees 14 | delta=23.45*sind(B); % declination angle in degrees 15 | elevation=asind(sind(delta)*sind(latitude)+cosd(delta)*cosd(latitude).*cosd(HRA)); 16 | % 17 | a1=sind(delta)*cosd(latitude); 18 | a2=cosd(delta)*sind(latitude); 19 | a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 20 | % % for i=1:length(HRA) 21 | % % if (HRA(i)<=0) 22 | % % azimuth(i)=a(i); 23 | % % elseif(HRA(i)>0) 24 | % % azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 25 | % % end 26 | % % end 27 | % 28 | for i=1:length(HRA) 29 | if (HRA(i)<=0 ) 30 | % if (1<=d<=115 && 230<=d<=365) HRA(i)>0 && 31 | azimuth(i)=a(i); 32 | elseif( 116 <= d && d <= 229 && HRA(i)>0) 33 | azimuth(i) =-a(i); %% VERY IMP: for some months like june azimuth = -a(1). otherwise time vs az-El graph will not be continuous 34 | else%if(HRA(i)>0 && 1<=d<=115 && 230<=d<=365) 35 | azimuth(i) =360-a(i); 36 | 37 | end 38 | end 39 | end 40 | %% Method 1 41 | % a = -cosd(delta).*sind(HRA)./cosd(elevation); 42 | % for i=1:length(HRA) 43 | % if (cosd(HRA(i)) >= tand(delta)/tand(latitude)) 44 | % azimuth(i) = 180-asind(a(i)); 45 | % else 46 | % azimuth(i) = 360+ asind(a(i)); 47 | % end 48 | % end 49 | %% Method 2 50 | % a1=sind(delta)*cosd(latitude); 51 | % a2=cosd(delta)*sind(latitude); 52 | % a=acosd((a1-a2.*cosd(HRA))./cosd(elevation)); 53 | % for i=1:length(HRA) 54 | % if (sind(HRA(i)) > 0) 55 | % azimuth(i) = 360 - a(i); 56 | % else 57 | % azimuth(i) = a(i); 58 | % end 59 | % end 60 | %% Method 3 61 | % a1=sind(delta)*cosd(latitude); 62 | % a2=cosd(delta)*sind(latitude).*cosd(HRA); 63 | % a3 = -cosd(delta).*sind(HRA); 64 | % azimuth = atan2d(a3,(a1-a2)); 65 | -------------------------------------------------------------------------------- /src/expt/error/areaintersection.m: -------------------------------------------------------------------------------- 1 | function [area]=areaintersection(set1, set2, resolution) 2 | 3 | % Function gives the approximate area of intersection of two polygons 4 | % whose vertices are given by set1 and set2. 5 | % 6 | % Note that implementation of this function requires the standard Matlab 7 | % function "inpolygon" 8 | % 9 | % STANDARD CALL 10 | % 11 | % area = areaintersection(set1, set2, resolution) 12 | % 13 | % 14 | % INPUTS 15 | % 16 | % set1 : an N x 2 matrix of the form (x1, y1; x1, y2; ...) giving the 17 | % vertices of a polygon 18 | % set2 : an M x 2 matrix of the form (x1, y1; x1, y2; ...) giving the 19 | % vertices of a second polygon 20 | % resolution : a positive scalar that determines the accuracy of the 21 | % approximation. The larger the value the greater the accuracy and the 22 | % longer the evaluation time. The matlab function "inpolygon" is the 23 | % real limiting factor in terms of execution time 24 | % 25 | % OUTPUT 26 | % 27 | % area : a scalar value that represents the area of intersection of the two 28 | % polynomials in standard units squared 29 | % 30 | % 31 | % 32 | % Paul Koprowski 2007 33 | % paulkoprowski@hotmail.com 34 | % 35 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 36 | 37 | % Checks the input arguments 38 | if nargin~=3 39 | error('wrong number of inputs'); 40 | end 41 | sset1=size(set1); 42 | sset2=size(set2); 43 | if sset1(1,1)<2 44 | error('set1 has too few rows'); 45 | end 46 | if sset2(1,1)<2 47 | error('set2 has too few rows'); 48 | end 49 | if sset1(1,2)<2 50 | error('set1 has too few columns'); 51 | end 52 | if sset2(1,2)<2 53 | error('set2 has too few columns'); 54 | end 55 | s=size(resolution); 56 | if s(1,1)~=1 57 | error('resolution must be a scalar'); 58 | end 59 | if s(1,2)~=1 60 | error('resolution must be a scalar'); 61 | end 62 | % creates a hull of all point in both polygons 63 | hset = [set1;set2]; 64 | % Extreme boundaries of hull 65 | maxx=max(hset(:,1)); 66 | minx=min(hset(:,1)); 67 | maxy=max(hset(:,2)); 68 | miny=min(hset(:,2)); 69 | 70 | % Generates matrix containing all points of both polygons and then some 71 | 72 | stepwidthx = (maxx-minx)/resolution; 73 | stepwidthy = (maxy-miny)/resolution; 74 | hullx = ones(resolution+1,1)*[minx:stepwidthx:maxx]; 75 | hully = ([miny:stepwidthy:maxy]')*ones(1,resolution+1); 76 | 77 | % Uses "inpolygon" function to determine points inside each polygon 78 | 79 | [poly1] = inpolygon(hullx,hully,set1(:,1),set1(:,2)); 80 | [poly2] = inpolygon(hullx,hully,set2(:,1),set2(:,2)); 81 | 82 | % Finds the intersection points of the two polygons 83 | 84 | int = and(poly1,poly2); 85 | 86 | % Multiplies the area of each box in the hull by the number of boxes 87 | % contained in each polygon to approximate the total area of intersection 88 | 89 | area = (stepwidthx*stepwidthy*sum(sum(int))); 90 | -------------------------------------------------------------------------------- /src/expt/az-el/Track_point.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | % Here the base coordinate system and the global are same and located at 6 | % the base of the Az-El heliostat. Hence O and O1 are the same points 7 | 8 | %% 9 | % ALL DIMENSIONS IN METERS 10 | O1G = [0;0;1.58]; % height of the heliostat or vert dist btw bcs and mcs. 11 | % Laser Coordintes. 12 | xl = 6.38; yl = -1.21; zl = 2.56; 13 | O1S = [xl;yl;zl]; 14 | % O1G + GS = O1S 15 | GS = (O1S - O1G)/norm((O1S - O1G),2) ; % unit incident ray 16 | 17 | % Coordintes of the Point to be tracked. 18 | xp = -3.545; yp = 2.74; zp = 3.10; 19 | O1P = [xp; yp; zp]; 20 | GP = (O1P - O1G)/norm((O1P - O1G),2); % unit reflected ray 21 | GN = (GP + GS)/norm((GP+GS),2); % unit mirror normal 22 | normal = GN; 23 | %% To find out the azimuth and elevation angle of rotation 24 | 25 | Motor_angle_Az_El = []; Az_El_count = []; 26 | N_azimuth= atan2d(normal(2),normal(1)); 27 | l=sqrt(normal(2)^2+normal(1)^2); 28 | N_elevation = atand(normal(3)/l); % ANGLE OF THE MIRRO NORMAL WRT HORIZONTAL. 29 | %IN THE AZ-EL HELIOSTAT MADE IN THE LAB, THE MOTOR ANGLE 30 | % SHOULD BE 90-N_elevation 31 | Motor_angle_azimuth = N_azimuth ; 32 | Motor_angle_elevation = 90-N_elevation ; 33 | 34 | counts_per_degree = 9091; 35 | % 1 degree = 9091 counts of the motor 36 | azi_count = counts_per_degree*Motor_angle_azimuth; 37 | ele_count = counts_per_degree*Motor_angle_elevation; 38 | 39 | Motor_angle_Az_El = cat(1,Motor_angle_Az_El,[Motor_angle_azimuth Motor_angle_elevation]) 40 | Az_El_count = cat(1, Az_El_count, [azi_count -ele_count]) % elevation is multiplied by a -ve sign 41 | % bcoz the Az-El heliostat made 42 | % in the lab rotates in -ve 43 | % direction if a positive value 44 | % is given and vice versa. This 45 | % is bcoz of the direction of 46 | % the axis chosen. 47 | 48 | %% GALIL 49 | g = actxserver('galil');%set the variable g to the GalilTools COM wrapper 50 | response = g.libraryVersion;%Retrieve the GalilTools library versions 51 | disp(response);%display GalilTools library version 52 | % g.address = '';%Open connections dialog box 53 | g.address = '192.168.1.1';%Open connections dialog box 54 | g.command('SH'); 55 | g.command('TPA=0'); 56 | g.command('TPB=0'); 57 | 58 | i=1; 59 | str1 = ['''PAA=',num2str(Az_El_count(i,1)),'''']; 60 | str2 = ['''PAB=',num2str(Az_El_count(i,2)),'''']; 61 | g.command(eval(str1)) 62 | g.command(eval(str2)) 63 | g.command('BGA'); 64 | g.command('BGB'); 65 | 66 | % 67 | % g.command('PAA=0'); 68 | % g.command('PAB=0'); 69 | % g.command('BGA'); 70 | % g.command('BGB'); 71 | % 72 | % g.command('PAA=50000'); 73 | % g.command('PAB=500000'); 74 | % g.command('BGA'); 75 | % g.command('BGB'); 76 | 77 | g.command('PRA=-45455'); 78 | g.command('BGA'); -------------------------------------------------------------------------------- /src/expt/3-rps/Encoder_calibration.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | % Encoder calibration 4 | time_fwd = [0 3 6 9 13 15 17 19 22 27]'; 5 | time_bwd = [0 5 8 10 12 14 18 21 24 27]'; 6 | 7 | %% 8 | set(0,'DefaultAxesFontName', 'Times New Roman') 9 | set(0,'DefaultAxesFontSize', 15) 10 | set(0,'DefaultAxesFontWeight', 'Bold') 11 | 12 | % Change default text fonts. 13 | set(0,'DefaultTextFontname', 'Times New Roman') 14 | set(0,'DefaultTextFontSize', 13) 15 | 16 | %% leg 1 17 | 18 | Dist_fwd_l1 = [0 8 15 22 30 34 39 44 51 62]'; % forward or outward motion of leg 1 19 | Enc_cnt_fwd_l1 = [0 674 1344 2039 2937 3423 3877 4309 4949 6130]'; % Encoder counts of leg 1 in forward motion 20 | 21 | Dist_bwd_l1 = [0 -12 -19 -24 -29 -33 -42 -49 -57 -64]'; % backward or inward motion of leg 1 22 | Enc_cnt_bwd_l1 = [0 -3006 -4779 -5921 -6996 -8143 -10507 -11677 -13364 -15032]'; % Encoder counts of leg 1 in bacward motion 23 | 24 | l1_fwd = fit(time_fwd,Dist_fwd_l1, 'poly1'); 25 | l1_bwd = fit(time_bwd,Dist_bwd_l1, 'poly1'); 26 | 27 | figure(1) 28 | subplot(1,2,1) 29 | plot(l1_fwd,time_fwd,Dist_fwd_l1,'r*') 30 | a = l1_fwd.p1; 31 | b = l1_fwd.p2; 32 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 33 | % polyfit_str qill be : y = 4*x + 2 34 | text(5,50,polyfit_str); 35 | ylabel('Distance (mm)') 36 | xlabel('time (s)') 37 | title ('Forward motion for leg 1') 38 | 39 | subplot(1,2,2) 40 | plot(l1_bwd,time_bwd,Dist_bwd_l1,'b*') 41 | a = l1_bwd.p1; 42 | b = l1_bwd.p2; 43 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 44 | % polyfit_str qill be : y = 4*x + 2 45 | text(5,-50,polyfit_str); 46 | xlabel('time (s)') 47 | ylabel('Distance (mm)') 48 | title ('Backward motion for leg 1') 49 | 50 | % Dist vs Encoder counts leg 1 51 | 52 | l1_fwd_en = fit(Dist_fwd_l1,Enc_cnt_fwd_l1, 'poly1'); 53 | l1_bwd_en = fit(Dist_bwd_l1,Enc_cnt_bwd_l1, 'poly1'); 54 | 55 | figure(2) 56 | subplot(1,2,1) 57 | plot(l1_fwd_en,Dist_fwd_l1,Enc_cnt_fwd_l1,'r*') 58 | a = l1_fwd_en.p1; 59 | b = l1_fwd_en.p2; 60 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 61 | % polyfit_str qill be : y = 4*x + 2 62 | text(5,3000,polyfit_str); 63 | xlabel('Distance (mm)') 64 | ylabel('Encoder counts') 65 | title ('Forward motion for leg 1') 66 | 67 | subplot(1,2,2) 68 | plot(l1_bwd_en,Dist_bwd_l1,Enc_cnt_bwd_l1,'b*') 69 | a = l1_bwd_en.p1; 70 | b = l1_bwd_en.p2; 71 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 72 | % polyfit_str qill be : y = 4*x + 2 73 | text(-65,-6000,polyfit_str); 74 | ylabel('Encoder counts') 75 | xlabel('Distance (mm)') 76 | title ('Backward motion for leg 1') 77 | 78 | 79 | %% leg 2 80 | 81 | Dist_fwd_l2 = [0 8 15 22 31 35 40 45 51 62]'; % forward or outward motion of leg 1 82 | Enc_cnt_fwd_l2 = [0 5225 10434 15584 22468 25810 29187 32489 37697 46377]'; % Encoder counts of leg 1 in forward motion 83 | 84 | Dist_bwd_l2 = [0 -11 -19 -23 -29 -34 -43 -49 -56 -63]'; % backward or inward motion of leg 1 85 | Enc_cnt_bwd_l2 = [0 -8989 -14366 -17885 -21349 -24934 -32256 -37652 -43102 -48522]'; % Encoder counts of leg 1 in bacward motion 86 | 87 | l2_fwd = fit(time_fwd,Dist_fwd_l2, 'poly1'); 88 | l2_bwd = fit(time_bwd,Dist_bwd_l2, 'poly1'); 89 | 90 | figure(3) 91 | subplot(1,2,1) 92 | plot(l2_fwd,time_fwd,Dist_fwd_l2,'r*') 93 | a = l2_fwd.p1; 94 | b = l2_fwd.p2; 95 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 96 | % polyfit_str qill be : y = 4*x + 2 97 | text(5,50,polyfit_str); 98 | ylabel('Distance (mm)') 99 | xlabel('time (s)') 100 | title ('Forward motion for leg 2') 101 | 102 | subplot(1,2,2) 103 | plot(l2_bwd,time_bwd,Dist_bwd_l2,'b*') 104 | a = l2_bwd.p1; 105 | b = l2_bwd.p2; 106 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 107 | % polyfit_str qill be : y = 4*x + 2 108 | text(5,-50,polyfit_str); 109 | xlabel('time (s)') 110 | ylabel('Distance (mm)') 111 | title ('Backward motion for leg 2') 112 | 113 | % Dist vs Encoder counts leg 2 114 | 115 | l2_fwd_en = fit(Dist_fwd_l2,Enc_cnt_fwd_l2, 'poly1'); 116 | l2_bwd_en = fit(Dist_bwd_l2,Enc_cnt_bwd_l2, 'poly1'); 117 | 118 | figure(4) 119 | subplot(1,2,1) 120 | plot(l2_fwd_en,Dist_fwd_l2,Enc_cnt_fwd_l2,'r*') 121 | a = l2_fwd_en.p1; 122 | b = l2_fwd_en.p2; 123 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 124 | % polyfit_str qill be : y = 4*x + 2 125 | text(5,3000,polyfit_str); 126 | xlabel('Distance (mm)') 127 | ylabel('Encoder counts') 128 | title ('Forward motion for leg 2') 129 | 130 | subplot(1,2,2) 131 | plot(l2_bwd_en,Dist_bwd_l2,Enc_cnt_bwd_l2,'b*') 132 | a = l2_bwd_en.p1; 133 | b = l2_bwd_en.p2; 134 | polyfit_str = ['y = ' num2str(a) ' *x + ' num2str(b)]; 135 | % polyfit_str qill be : y = 4*x + 2 136 | text(-65,-6000,polyfit_str); 137 | ylabel('Encoder counts') 138 | xlabel('Distance (mm)') 139 | title ('Backward motion for leg 2') 140 | 141 | 142 | 143 | -------------------------------------------------------------------------------- /src/expt/3-rps/TEST.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | home 5 | global OP t zc Rot rp ard 6 | 7 | % The base of the Az-El heliostat is the origin of the global co-ordinate 8 | % system. 9 | rb=0.400; 10 | rp=0.250; 11 | xl = 6.385; yl= -1.21; zl = 2.56; % LASER co-ordinates one fixed on the window frame 12 | zc=1.64; 13 | tx = 10*.3048; ty =0.0; tz=0; t=[tx;ty;tz]; 14 | xl_3rps = xl-tx; yl_3rps = yl-ty; zl_3rps = zl-zc; 15 | 16 | % [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are co-ordinates of the sun 17 | 18 | OS1=[xl_3rps;yl_3rps;zl_3rps]; 19 | unit_sun = OS1/norm(OS1,2); 20 | 21 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 22 | O1Rs=[O1R1 O1R2 O1R3]; 23 | 24 | GS1=[rp;0;0]; GS2=[-.5*rp;sqrt(3)*rp/2;0]; GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 25 | GSs=[GS1 GS2 GS3]; 26 | 27 | gamma=0; % the angle x_b(base coordinate system) makes with local-east 28 | Rot=[ cosd(gamma) -sind(gamma) 0; 29 | sind(gamma) cosd(gamma) 0; 30 | 0 0 1]; 31 | x0 = [0.8;0.61;0.5;-0.01;0.015]; 32 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 33 | % options = optimoptions('fsolve','Display','iter','TolX',1e-9); 34 | % options = optimoptions('fsolve','Jacobian','off'); 35 | 36 | %% 37 | 38 | % Coordintes of the Point to be tracked. 39 | xp = -3.545; yp = 2.74; zp = 3.10; 40 | OP = [xp; yp; zp]; 41 | Act_req =[]; 42 | l10 = 1.645; l20 = 1.653; l30 = 1.650; 43 | % O1P = OP- t; 44 | for i=1:1%length(OS_d) 45 | % fhandle=@(x)for_mainprog_3RPS(x,OS_d(:,i)); 46 | fhandle=@(x)for_mainprog_3RPS(x,unit_sun); 47 | 48 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 49 | x0=xval(:,i); 50 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 51 | B0t(:,i)=[xc;yc;zc]; 52 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 53 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 54 | g=O1P_bcs-O1G; 55 | GP=g/norm(g,2); 56 | % GS=Rot'*OS_d(:,i); % Sun vector in bcs 57 | GS=Rot'*unit_sun; % Sun vector in bcs 58 | normal=(GS+GP)/norm((GS+GP),2); 59 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 60 | n2=-yc/rp; 61 | o1=n2; 62 | o2=n1-(2*xc/rp); 63 | Trans_matr = [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 64 | n2 o2 a2 yc; 65 | n3 o3 a3 zc; 66 | 0 0 0 1] ; 67 | B0p1 = Trans_matr*[GS1;1]; % w.r.t base coordinate system 68 | l1_dir = B0p1-[O1R1;1]; 69 | l1(i,:)=norm(l1_dir,2); 70 | 71 | B0p2 = Trans_matr*[GS2;1]; 72 | l2_dir = B0p2-[O1R2;1]; 73 | l2(i,:)=norm(l2_dir,2); 74 | 75 | B0p3 = Trans_matr*[GS3;1]; 76 | l3_dir = B0p3-[O1R3;1]; 77 | l3(i,:)=norm(l3_dir,2); 78 | 79 | % Act_req = cat(1, Act_req, [tme(i), 1000*(l1(i)-l10) 1000*(l2(i)-l20) 1000*(l3(i)-l30)]); 80 | Act_req = cat(1, Act_req, [1000*(l1(i)-l10) 1000*(l2(i)-l20) 1000*(l3(i)-l30)]); 81 | end 82 | Act_tme = [Act_req(:,1)/2.4 Act_req(:,2)/2.4 Act_req(:,3)/2.4 ]; 83 | L = [l1 l2 l3] 84 | 85 | act = Act_tme; 86 | [rw, cl] = size(act); 87 | [sort_act, Ind_act] = sort(abs(act),2); 88 | 89 | 90 | 91 | delete(instrfind({'PORT'},{'COM7'})) 92 | ard = arduino('COM7'); 93 | % Pins 4,5,6,7,8,9 are PWM pins. Motor 1 uses 4 and 5, Motor 2 uses 6 and 7 94 | % and Motor 3 uses 8 and 9. 95 | % Pins 31,32,33,34,35 are digital pins. Motor 1 uses 31 and NOT gate, Motor 96 | % 2 uses 32 and 33 and Motor 3 uses 34 and 35 97 | % Pins 2,3,18,19,20,21 are used for encoder input signals. Motor 1 uses 2 98 | % and 3, Motor 2 uses 18 and 19 and Motor 3 uses 20 and 21 99 | ard.pinMode(4,'OUTPUT'); 100 | ard.pinMode(5,'OUTPUT'); 101 | ard.pinMode(6, 'OUTPUT'); 102 | ard.pinMode(7,'OUTPUT'); 103 | ard.pinMode(8,'OUTPUT'); 104 | ard.pinMode(9, 'OUTPUT'); 105 | 106 | ard.pinMode(30,'OUTPUT'); 107 | ard.pinMode(31,'OUTPUT'); 108 | ard.pinMode(32,'OUTPUT'); 109 | ard.pinMode(33, 'OUTPUT'); 110 | ard.pinMode(34,'OUTPUT'); 111 | ard.pinMode(35,'OUTPUT'); 112 | 113 | 114 | for i=1:rw 115 | tic 116 | while(toc <= sort_act(i,1)) 117 | RunMotor1(act(i,1)) 118 | RunMotor2(act(i,2)) 119 | RunMotor3(act(i,3)) 120 | end 121 | MOTORSTOP 122 | tic 123 | while (toc <= (sort_act(i,2)-sort_act(i,1))) 124 | if Ind_act(i,2) == 1 125 | RunMotor1(act(i,1)) 126 | elseif Ind_act(i,2) == 2 127 | RunMotor2(act(i,2)) 128 | elseif Ind_act(i,2) == 3 129 | RunMotor3(act(i,3)) 130 | end 131 | 132 | if Ind_act(i,3) == 1 133 | RunMotor1(act(i,1)) 134 | elseif Ind_act(i,3) == 2 135 | RunMotor2(act(i,2)) 136 | elseif Ind_act(i,3) == 3 137 | RunMotor3(act(i,3)) 138 | end 139 | end 140 | MOTORSTOP 141 | tic 142 | while (toc <= (sort_act(i,3)-sort_act(i,2))) 143 | if Ind_act(i,3) == 1 144 | RunMotor1(act(i,1)) 145 | elseif Ind_act(i,3) == 2 146 | RunMotor2(act(i,2)) 147 | elseif Ind_act(i,3) == 3 148 | RunMotor3(act(i,3)) 149 | end 150 | end 151 | MOTORSTOP 152 | pause 153 | end 154 | -------------------------------------------------------------------------------- /src/expt/3-rps/ICER_3RPS_Expt.m: -------------------------------------------------------------------------------- 1 | %inputs needed are station co-ordinates longitude,latitude in degrees,min,sec ... 2 | %altitude in kms and time of proposed sun visibility in year,month,day in Christian era, 3 | %hr,min and sec in GMT (NOT BASED ON LOCAL TIME). 4 | %Input the following 5 | clear all 6 | home 7 | close all 8 | clc 9 | global OP t zc Rot rp 10 | 11 | %% The following part of the code is used to mark point on the lab wall to show sun vector 12 | % Bangalore Latitude 13 | lad=12; 14 | lam=58; 15 | las=13; 16 | 17 | % Bangalore Longitude 18 | lod=77; 19 | lom=33; 20 | los=37; 21 | 22 | latitude=lad+(lam+las/60)/60; % in degrees 23 | longitude=lod+(lom+los/60)/60; % in degrees 24 | 25 | year=2016; 26 | month=12; 27 | day=20; 28 | d=date(year,month,day); 29 | 30 | time_incre=900; % in seconds or in increments of 15 minutes 31 | LT=0:time_incre:24*3600; 32 | 33 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 34 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 35 | OS=[xs;ys;zs]; 36 | 37 | t_900 = 9/time_incre*3600+1; 38 | t_915 = t_900 + 1; 39 | t_930 = t_900 + 2; 40 | t_945 = t_900 + 3; 41 | t_1000 = 10/time_incre*3600+1; 42 | t_1015 = t_1000 + 1; 43 | t_1030 = t_1000 + 2; 44 | t_1045 = t_1000 + 3; 45 | 46 | t_1100 = 11/time_incre*3600+1; 47 | t_1115 = t_1100 + 1; 48 | t_1130 = t_1100 + 2; 49 | t_1145 = t_1100 + 3; 50 | 51 | t_1200 = 12/time_incre*3600+1; 52 | t_1215 = t_1200 + 1; 53 | t_1230 = t_1200 + 2; 54 | t_1245 = t_1200 + 3; 55 | 56 | t_100 = 13/time_incre*3600+1; 57 | t_115 = t_100 + 1; 58 | t_130 = t_100 + 2; 59 | t_145 = t_100 + 3; 60 | 61 | t_200 = 14/time_incre*3600+1; 62 | t_215 = t_200 + 1; 63 | t_230 = t_200 + 2; 64 | t_245 = t_200 + 3; 65 | t_300 = t_200 + 4; 66 | t_315 = t_200 + 5; 67 | t_330 = t_200 + 6; 68 | t_345 = t_200 + 7; 69 | t_400 = 16/time_incre*3600+1; 70 | t_415 = t_400 + 1; 71 | t_430 = t_400 + 2; 72 | t_445 = t_400 + 3; 73 | t_500 = 17/time_incre*3600+1; 74 | t_515 = t_500 + 1; 75 | t_530 = t_500 + 2; 76 | 77 | %% Plot of azimuth and elevation angles 78 | figure(1) 79 | plot(LT(t_900:t_415)/3600,elevation((t_900:t_415)),'r','LineWidth',4) 80 | hold on 81 | plot(LT(t_900:t_415)/3600,azimuth((t_900:t_415)),'b','LineWidth',4) 82 | xlabel('Time \it{t / hr}') 83 | ylabel('Sun vector angles \it{\theta / (^0)}') 84 | title('Azimuth and Elevation angles of the sun') 85 | lgnd=legend('Elevation',' Azimuth'); 86 | grid on 87 | 88 | %% 89 | rb=0.400; 90 | rp=0.250; 91 | zc=1.64; 92 | a = 0; b = 0; c = 6.72; % wrt TCS 93 | OP = [a;b;c]; % wrt TCS; O is the origin of TCS which is R[10deg, Z] wrt gcs. 94 | OO1 = [-14;3.45;0]; 95 | t = OO1; 96 | 97 | % Rot = eye(3); 98 | incl = 10; % angle by which TCS and gcs are rotated 99 | Rot_incl = [cosd(incl) -sind(incl) 0; 100 | sind(incl) cosd(incl) 0; 101 | 0 0 1]; 102 | 103 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 104 | O1Rs=[O1R1 O1R2 O1R3]; 105 | 106 | GS1=[rp;0;0]; GS2=[-.5*rp;sqrt(3)*rp/2;0]; GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 107 | GSs=[GS1 GS2 GS3]; 108 | 109 | gamma=0; % the angle x_b(base coordinate system) makes with local-east 110 | Rot=[ cosd(gamma) -sind(gamma) 0; 111 | sind(gamma) cosd(gamma) 0; 112 | 0 0 1]; 113 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; 114 | x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 115 | % x0 = [-.7;-.6;-0.1;-0.7;0.01;-0.05]; 116 | % x0=[0.25; 0.6; 0.4; 0.35; 0.56]; 117 | % x0 = [1.7;.6;-1;.7;.1;.5;0.08;1]; 118 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 119 | % options = optimoptions('fsolve','Display','iter','TolX',1e-9); 120 | % options = optimoptions('fsolve','Jacobian','off'); 121 | 122 | %% 123 | % OS_d = [OS(:,t_700) OS(:,t_800) OS(:,t_900) OS(:,t_1000) OS(:,t_1100) OS(:,t_1200) OS(:,t_100) OS(:,t_200) OS(:,t_300) OS(:,t_400) OS(:,t_500)];% OS(:,t_600)]; 124 | OS_d = [OS(:,t_1000) OS(:,t_1015) OS(:,t_1030) OS(:,t_1045) ... 125 | OS(:,t_1100) OS(:,t_1115) OS(:,t_1130) OS(:,t_1145) ... 126 | OS(:,t_1200) OS(:,t_1215) OS(:,t_1230) OS(:,t_1245) ... 127 | OS(:,t_100) OS(:,t_115) OS(:,t_130) OS(:,t_145) ... 128 | OS(:,t_200) OS(:,t_215) OS(:,t_230) OS(:,t_245) ... 129 | OS(:,t_300) OS(:,t_315) OS(:,t_330) OS(:,t_345) ... 130 | OS(:,t_400) OS(:,t_415) OS(:,t_430) OS(:,t_445) ... 131 | OS(:,t_500)]; 132 | 133 | Act_req = []; 134 | % tme = [7 8 9 10 11 12 1 2 3 4 5]; 135 | tme = [10 10.15 10.30 10.45 11.00 11.15 11.30 11.45 ... 136 | 12.00 12.15 12.30 12.45 1.00 1.15 1.30 1.45 2.00 ... 137 | 2.15 2.30 2.45 3.00 3.15 3.30 3.45 4.00 4.15 4.30 4.45 5.00]; 138 | 139 | l10 = 1.645; l20 = 1.653; l30 = 1.650; 140 | 141 | for i=1:length(OS_d) 142 | fhandle=@(x)for_mainprog_3RPS(x,OS_d(:,i)); 143 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 144 | x0=xval(:,i); 145 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 146 | B0t(:,i)=[xc;yc;zc]; 147 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 148 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 149 | g=O1P_bcs-O1G; 150 | GP=g/norm(g,2); 151 | GS=Rot'*OS_d(:,i); % Sun vector in bcs 152 | normal=(GS+GP)/norm((GS+GP),2); 153 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 154 | n2=-yc/rp; 155 | o1=n2; 156 | o2=n1-(2*xc/rp); 157 | Trans_matr = [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 158 | n2 o2 a2 yc; 159 | n3 o3 a3 zc; 160 | 0 0 0 1] ; 161 | B0p1 = Trans_matr*[GS1;1]; % w.r.t base coordinate system 162 | l1_dir = B0p1-[O1R1;1]; 163 | l1(i,:)=norm(l1_dir,2); 164 | 165 | B0p2 = Trans_matr*[GS2;1]; 166 | l2_dir = B0p2-[O1R2;1]; 167 | l2(i,:)=norm(l2_dir,2); 168 | 169 | B0p3 = Trans_matr*[GS3;1]; 170 | l3_dir = B0p3-[O1R3;1]; 171 | l3(i,:)=norm(l3_dir,2); 172 | 173 | Act_req = cat(1, Act_req, [tme(i), 1000*(l1(i)-l10) 1000*(l2(i)-l20) 1000*(l3(i)-l30)]); 174 | end 175 | % in 1 second the actuator moves 2.4 mm when the speed is 200(bits). 0-0 velocity and 255-max velocity 176 | Act_tme = [Act_req(:,1) Act_req(:,2)/2.4 Act_req(:,3)/2.4 Act_req(:,4)/2.4 ]; 177 | L = [l1 l2 l3]; 178 | 179 | for i=1:length(Act_tme)-1 180 | act(i,:) = Act_tme(i+1,:) - Act_tme(i,:); 181 | end 182 | act(:,1) = []; 183 | act = cat(1, Act_tme(1,2:end), act); 184 | Act_time = [Act_tme(4,:); Act_tme(end,:); Act_tme(end,:)-Act_tme(4,:) ]; 185 | 186 | 187 | %% Arduino 188 | global ard 189 | delete(instrfind({'PORT'},{'COM7'})) 190 | ard = arduino('COM7'); 191 | % Pins 4,5,6,7,8,9 are PWM pins. Motor 1 uses 4 and 5, Motor 2 uses 6 and 7 192 | % and Motor 3 uses 8 and 9. 193 | % Pins 31,32,33,34,35 are digital pins. Motor 1 uses 31 and NOT gate, Motor 194 | % 2 uses 32 and 33 and Motor 3 uses 34 and 35 195 | % Pins 2,3,18,19,20,21 are used for encoder input signals. Motor 1 uses 2 196 | % and 3, Motor 2 uses 18 and 19 and Motor 3 uses 20 and 21 197 | ard.pinMode(4,'OUTPUT'); 198 | ard.pinMode(5,'OUTPUT'); 199 | ard.pinMode(6, 'OUTPUT'); 200 | ard.pinMode(7,'OUTPUT'); 201 | ard.pinMode(8,'OUTPUT'); 202 | ard.pinMode(9, 'OUTPUT'); 203 | 204 | ard.pinMode(30,'OUTPUT'); 205 | ard.pinMode(31,'OUTPUT'); 206 | ard.pinMode(32,'OUTPUT'); 207 | ard.pinMode(33, 'OUTPUT'); 208 | ard.pinMode(34,'OUTPUT'); 209 | ard.pinMode(35,'OUTPUT'); 210 | 211 | [rw, cl] = size(act); 212 | [sort_act, Ind_act] = sort(abs(act),2); 213 | for i=1:rw 214 | tic 215 | while(toc <= sort_act(i,1)) 216 | RunMotor1(act(i,1)) 217 | RunMotor2(act(i,2)) 218 | RunMotor3(act(i,3)) 219 | end 220 | MOTORSTOP 221 | % fprintf(fileID, 'Elapsed Time is %8.3f \n',(toc)) 222 | % pause(1) 223 | % fprintf(fileID, 'AfterPause,Elapsed Time is %4.2f \n',toc) 224 | tic 225 | while (toc <= (sort_act(i,2)-sort_act(i,1))) 226 | if Ind_act(i,2) == 1 227 | RunMotor1(act(i,1)) 228 | elseif Ind_act(i,2) == 2 229 | RunMotor2(act(i,2)) 230 | elseif Ind_act(i,2) == 3 231 | RunMotor3(act(i,3)) 232 | end 233 | 234 | if Ind_act(i,3) == 1 235 | RunMotor1(act(i,1)) 236 | elseif Ind_act(i,3) == 2 237 | RunMotor2(act(i,2)) 238 | elseif Ind_act(i,3) == 3 239 | RunMotor3(act(i,3)) 240 | end 241 | end 242 | MOTORSTOP 243 | tic 244 | while (toc <= (sort_act(i,3)-sort_act(i,2))) 245 | if Ind_act(i,3) == 1 246 | RunMotor1(act(i,1)) 247 | elseif Ind_act(i,3) == 2 248 | RunMotor2(act(i,2)) 249 | elseif Ind_act(i,3) == 3 250 | RunMotor3(act(i,3)) 251 | end 252 | end 253 | MOTORSTOP 254 | % fprintf(fileID, 'Elapsed Time is %4.2f \n',toc) 255 | pause(910) 256 | % fprintf(fileID, 'AfterPause,Elapsed Time is %4.2f \n',toc) 257 | end 258 | 259 | 260 | 261 | 262 | -------------------------------------------------------------------------------- /src/AzimuthElevation/TEST.m: -------------------------------------------------------------------------------- 1 | % This program uses fsolve to solve for AZ-EL case. Since there are only 2 | % two rotations happening, viz. Rz and Ry, in AZ-EL case, the R(3,2) element of the rotation 3 | % matrix remains zero. The sun vector, the reflected ray are completely 4 | % known(since the center is fixed). Hence, the direction cosines of the 5 | % normal can be found out. Therefore there are only 5 unknowns, viz., 6 | % n1,n2,n3,o1 and o2 which are related by 5 equations, viz., n.n=1, o.o=1, 7 | % n.a=0, n.o=0, a.o=0 ; Fsolve is used to solve this. It should be noted 8 | % that depending upon the values of the initial condition, x0, the solution 9 | % changes 10 | clear all 11 | home 12 | close all 13 | global O1M OP Rot_gam OO1 14 | % Bangalore Latitude 15 | lad=12; 16 | lam=58; 17 | las=13; 18 | % Bangalore Longitude 19 | lod=77; 20 | lom=33; 21 | los=37; 22 | 23 | latitude=lad+(lam+las/60)/60; % in degrees 24 | longitude=lod+(lom+los/60)/60; % in degrees 25 | 26 | a=0;b=0;c=65; % Receiver coordinates wrt gcs 27 | OP = [a;b;c]; % Receiver vector 28 | year=2016; 29 | month=11; 30 | day=1; 31 | d=date(year,month,day); 32 | 33 | time_incre=10*60; % seconds 34 | LT=0*3600:time_incre:24*3600; 35 | 36 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); % aximuth and elevation of sun 37 | [xs, ys, zs]=sunposition(elevation, azimuth); % x2,y2,z2 are coordinates of the sun 38 | OS=[xs;ys;zs]; % Sun vector 39 | rad=100; % radius of the mirror from the origin 40 | psi=30; % psi is the angle the heliostat centre makes with the local east(X) axis 41 | zG=.5; % height of the heliostat 42 | 43 | KJ = -.25; % distance between the mirror center and the top platform center 44 | 45 | O1M = [0;0;zG]; % vector from the base origin to the mirror origin described wrt bcs 46 | OO1=[rad*cosd(psi);rad*sind(psi);0]; % vector from the global origin to the base origin 47 | v=9/time_incre*3600+1; % start time for the plots 48 | w=16/time_incre*3600+1; % end time for the plots 49 | o3=0; % three-two element of the rotation matrix 50 | 51 | gamma = 0; % the angle by which the x_b is inclined towards X (East) 52 | Rot_gam=[ cosd(gamma) -sind(gamma) 0; 53 | sind(gamma) cosd(gamma) 0; 54 | 0 0 1]; 55 | 56 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; 57 | x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 58 | % x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 59 | % x0 = [0.2903; -0.4146; -0.8625; 0.8192; 0.5735]; % for 3rd quadrant 60 | % x0 = [0.2903; -0.4146; 0.5625; -0.8192; 0.5735]; 61 | % x0 = [0.1761;-0.5876;-0.7897;0.9579;0.2871]; % Initial guesses for n1,n2,n3,o1 and o2 62 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 63 | for i=1:length(v:w) 64 | zz=v-1+i; 65 | fhandle=@(x)for_mainprog_AZ_EL_3_UPU(x,OS(:,zz)); 66 | [x,fval] = fsolve(fhandle,x0,options); 67 | x0=x; 68 | n1=x(1);n2=x(2);n3=x(3);o1=x(4);o2=x(5); 69 | O1P_bcs=Rot_gam'*(OP-OO1); % O1P_bcs = O1P described in bcs 70 | g=O1P_bcs-O1M; % wrt bcs 71 | GP=g/norm(g,2); % unit reflected ray wrt bcs 72 | GS=Rot_gam'*OS(:,zz); % Sun vector in bcs 73 | GN=(GS+GP)/norm((GS+GP),2); % unit mirror normal wrt bcs 74 | a1=GN(1); a2=GN(2); a3=GN(3); 75 | 76 | Trans_matr(:,:,i)= [n1 o1 a1 O1M(1); % transformation which takes mirror to base coordinate system 77 | n2 o2 a2 O1M(2); 78 | n3 o3 a3 O1M(3); 79 | 0 0 0 1] ; 80 | Rot(:,:,i)= [n1 o1 a1 ; % rotation which takes mirror to base coordinate system 81 | n2 o2 a2 ; 82 | n3 o3 a3]; 83 | % R(:,:,i) = Rot_gam*Rot(:,:,i); % rotation which takes mirror to global coordinate system 84 | O1G(:,i) = O1M+KJ*Rot(:,3,i); % Vector from the base origin to the origin of the top platform 85 | % plot3(O1G(1,i),O1G(2,i),O1G(3,i),'r*') 86 | % hold on 87 | end 88 | 89 | %% Leg lengths 90 | figure(1) 91 | rb=.5; % circum radius of the base equilateral triangle, B1B2/sqrt(3) in meters 92 | rp=0.25; % circum radius of the platform,P1P2/sqrt(3)in meters 93 | 94 | ss=0.08; % square mirror half length 95 | % M is the centre of the mirror and M1, M2, M3, and M4 are the corners 96 | MM1=[ss;ss;0]; MM2=[-ss;ss;0];MM3=[-ss;-ss;0]; MM4=[ss;-ss;0]; 97 | 98 | O1R1=[rb;0;0];O1R2=[-0.5*rb;sqrt(3)*rb/2;0];O1R3=[-0.5*rb;-sqrt(3)*rb/2;0]; 99 | O1Rs=[O1R1 O1R2 O1R3]; 100 | 101 | GS1=[rp;0;0];GS2=[-0.5*rp;sqrt(3)*rp/2;0];GS3=[-0.5*rp;-sqrt(3)*rp/2;0]; 102 | GSs=[GS1 GS2 GS3]; 103 | for i=1:length(Rot) 104 | 105 | O1S1=Rot(:,:,i)*GS1+O1G(:,i); % w.r.t base coordinate system 106 | l1_dir(:,i)=O1S1-O1R1; 107 | l1(i,:)=norm(l1_dir(:,i)); 108 | 109 | O1S2=Rot(:,:,i)*GS2+O1G(:,i); 110 | l2_dir(:,i)=O1S2-O1R2; 111 | l2(i,:)=norm(l2_dir(:,i)); 112 | 113 | O1S3=Rot(:,:,i)*GS3+O1G(:,i); 114 | l3_dir(:,i)=O1S3-O1R3; 115 | l3(i,:)=norm(l3_dir(:,i)); 116 | end 117 | 118 | plot(LT(v:w)/3600, l1*1000,'r','LineWidth', 2) 119 | hold on 120 | plot(LT(v:w)/3600, l2*1000,'g','LineWidth', 2) 121 | plot(LT(v:w)/3600, l3*1000,'b','LineWidth', 2) 122 | xlabel('Time, hrs') 123 | ylabel('leg length, mm') 124 | % title('leg length Vs Time Az-El of 3-UPU') 125 | legend('leg1','leg2', 'leg3') 126 | 127 | leg_vec = [LT(v:w)'/3600 l1*1000 l2*1000 l3*1000]; 128 | leg_vec(1:40,:) = []; 129 | load('Rot_mat_TA.mat') 130 | for i=1:length(Rot) 131 | Rzz(:,:,i) = Rot(:,:,i)'*Rot_TA(:,:,i); 132 | angle(i,:) = atan2d(Rzz(2,1,i),Rzz(1,1,i)); 133 | end 134 | 135 | %% For Animation 136 | TransmatrN=[Rot_gam,OO1;[0 0 0 1]]; % transformation which takes base to global coordinate system 137 | base=TransmatrN*[O1Rs;[1 1 1]]; 138 | writerObj=VideoWriter('3UPU.avi'); 139 | % writerObj.FrameRate = 9; 140 | open(writerObj); 141 | figure(2) 142 | for i=1:length(Trans_matr)% 143 | % for i =1:5 144 | xxx=base(1,:); % wrt global coordinate system 145 | yyy=base(2,:); % wrt global coordinate system 146 | zzz=base(3,:); % wrt global coordinate system 147 | fill3(xxx,yyy,zzz,'r') 148 | hold on 149 | OM = OO1 + Rot_gam*O1M; 150 | OM1 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM1); 151 | OM2 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM2); 152 | OM3 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM3); 153 | OM4 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM4); 154 | 155 | Mirror=[OM1 OM2 OM3 OM4]; 156 | xm=Mirror(1,:); 157 | ym=Mirror(2,:); 158 | zm=Mirror(3,:); 159 | % fill3(xm,ym,zm,'g') % top platform or mirror 160 | 161 | OS1=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS1); 162 | OS2=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS2); 163 | OS3=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS3); 164 | top_triangle=[OS1 OS2 OS3]; 165 | xt=top_triangle(1,:); 166 | yt=top_triangle(2,:); 167 | zt=top_triangle(3,:); 168 | fill3(xt,yt,zt,'c') % top triangle 169 | 170 | TRANSMATR=Rot_gam*Rot(:,:,i); 171 | plot3([xt(1) xxx(1)],[yt(1) yyy(1)],[zt(1) zzz(1)],'--k','LineWidth',2.5) 172 | plot3([xt(2) xxx(2)],[yt(2) yyy(2)],[zt(2) zzz(2)],'--k','LineWidth',2.5) 173 | plot3([xt(3) xxx(3)],[yt(3) yyy(3)],[zt(3) zzz(3)],'--k','LineWidth',2.5) 174 | % %% changing wrt global coordinate system 175 | 176 | kons=1;kon=.5;zz=v-1+i; 177 | normal=plot3([OM(1,1) OM(1,1)+kon*TRANSMATR(1,3)],[OM(2,1) OM(2,1)+kon*TRANSMATR(2,3)],[OM(3,1) OM(3,1)+kon*TRANSMATR(3,3)],'r','LineWidth',2); 178 | % proj=plot3([B0t(1,i) B0t(1,i)+kon*Trans_matr(1,3,i)],[B0t(2,i) B0t(2,i)+kon*Trans_matr(2,3,i)],[0 0],'r','LineWidth',2); 179 | 180 | sunposition=plot3([OM(1,1) OM(1,1)+kons*xs(zz)],[OM(2,1) OM(2,1)+kons*ys(zz)],[OM(3,1) OM(3,1)+kons*zs(zz)],'k','LineWidth',2); % position of the sun at every time_incre looking from plate coordinte system 181 | plot3(OM(1,1)+kons*xs(v:zz),OM(2,1)+kons*ys(v:zz),OM(3,1)+kons*zs(v:zz),'c*'); % position of the sun at every time_incre looking from plate coordinte system 182 | 183 | %% 184 | kjs =.5; 185 | plot3([OM(1,1) OM(1,1)+kjs*GP(1,1)],[OM(2,1) OM(2,1)+kjs*GP(2,1)],[OM(3,1) OM(3,1)+kjs*GP(3,1)],'b','LineWidth',2); 186 | 187 | %% 188 | % % coordintes of the receiver 189 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'b','LineWidth',2); 190 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'k*'); 191 | % plot3([a a],[b b],[0 c],'b'); 192 | 193 | % xlim([10 30]); ylim([40 50]); zlim([0 20]); 194 | % xlim([-50 120]); ylim([0 30]); zlim([0 50]); 195 | mg = .3; 196 | axis([OO1(1)-mg OO1(1)+mg OO1(2)-mg OO1(2)+mg 0 1]); 197 | % axis([7 10 4.5 5.5 0 1.5]); 198 | % view(82,6) % to see the 199 | % Receiver 200 | axis square 201 | % axis tight 202 | % view(-26,34); 203 | grid on 204 | xlabel('East(x)') 205 | ylabel('North(y)') 206 | zlabel('Zenith(p)') 207 | hold off 208 | M(i)=getframe(gcf); 209 | writeVideo(writerObj,M(i)); 210 | % pause(0.1) 211 | end 212 | close(writerObj) -------------------------------------------------------------------------------- /src/expt/az-el/ICER_expt.m: -------------------------------------------------------------------------------- 1 | %inputs needed are station co-ordinates longitude,latitude in degrees,min,sec ... 2 | %altitude in kms and time of proposed sun visibility in year,month,day in Christian era, 3 | %hr,min and sec in GMT (NOT BASED ON LOCAL TIME). 4 | %Input the following 5 | clear all 6 | home 7 | close all 8 | clc 9 | %% The following part of the code is used to mark point on the lab wall to show sun vector 10 | % Bangalore Latitude 11 | lad=12; 12 | lam=58; 13 | las=13; 14 | 15 | % Bangalore Longitude 16 | lod=77; 17 | lom=33; 18 | los=37; 19 | 20 | latitude=lad+(lam+las/60)/60; % in degrees 21 | longitude=lod+(lom+los/60)/60; % in degrees 22 | 23 | year=2016; 24 | month=11; 25 | day=10; 26 | d=date(year,month,day); 27 | 28 | time_incre=900; % in seconds or in increments of 15 minutes 29 | LT=0:time_incre:24*3600; 30 | 31 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 32 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 33 | OS=[xs;ys;zs]; 34 | 35 | t_900 = 9/time_incre*3600+1; 36 | t_915 = t_900 + 1; 37 | t_930 = t_900 + 2; 38 | t_945 = t_900 + 3; 39 | t_1000 = 10/time_incre*3600+1; 40 | t_1015 = t_1000 + 1; 41 | t_1030 = t_1000 + 2; 42 | t_1045 = t_1000 + 3; 43 | 44 | t_1100 = 11/time_incre*3600+1; 45 | t_1115 = t_1100 + 1; 46 | t_1130 = t_1100 + 2; 47 | t_1145 = t_1100 + 3; 48 | 49 | t_1200 = 12/time_incre*3600+1; 50 | t_1215 = t_1200 + 1; 51 | t_1230 = t_1200 + 2; 52 | t_1245 = t_1200 + 3; 53 | 54 | t_1300 = 13/time_incre*3600+1; 55 | t_1315 = t_1300 + 1; 56 | t_1330 = t_1300 + 2; 57 | t_1345 = t_1300 + 3; 58 | 59 | t_200 = 14/time_incre*3600+1; 60 | t_215 = t_200 + 1; 61 | t_230 = t_200 + 2; 62 | t_245 = t_200 + 3; 63 | t_300 = t_200 + 4; 64 | t_315 = t_200 + 5; 65 | t_330 = t_200 + 6; 66 | t_345 = t_200 + 7; 67 | t_400 = 16/time_incre*3600+1; 68 | t_415 = t_400 + 1; 69 | t_430 = t_400 + 2; 70 | t_445 = t_400 + 3; 71 | t_500 = 17/time_incre*3600+1; 72 | t_515 = t_500 + 1; 73 | t_530 = t_500 + 2; 74 | %% Plot of azimuth and elevation angles 75 | figure(1) 76 | plot(LT(t_900:t_415)/3600,elevation((t_900:t_415)),'r','LineWidth',4) 77 | hold on 78 | plot(LT(t_900:t_415)/3600,azimuth((t_900:t_415)),'b','LineWidth',4) 79 | xlabel('Time \it{t / hr}') 80 | ylabel('Sun vector angles \it{\theta / (^0)}') 81 | title('Azimuth and Elevation angles of the sun') 82 | lgnd=legend('Elevation',' Azimuth'); 83 | grid on 84 | %% 85 | Motor_angle_Az_El =[]; Az_El_count =[]; 86 | zG = 1.58; 87 | zz = t_900; 88 | 89 | a = 0; b = 0; c = 6.72; % wrt TCS 90 | OP = [a;b;c]; % wrt TCS; O is the origin of TCS which is R[10deg, Z] wrt gcs. 91 | OO1 = [-14;5.45;0]; 92 | O1G = [0;0;zG]; % Heliostat centre. same in both TCS,gcs,bcs 93 | OG = (OO1+O1G); 94 | GP =(OP-OG)/norm((OP-OG),2); 95 | % Rot = eye(3); 96 | incl = 10; % angle by which TCS and gcs are rotated 97 | Rot = [cosd(incl) -sind(incl) 0; 98 | sind(incl) cosd(incl) 0; 99 | 0 0 1]; 100 | 101 | for i=1:length(t_1200:t_530) 102 | GS=Rot'*OS(:,zz); 103 | n = GS + GP; 104 | normal=n/norm(n,2); 105 | xn=normal(1); yn=normal(2); zn=normal(3); qq=sqrt(xn^2+yn^2); 106 | rotmat =[xn*zn/qq -yn/qq xn ;yn*zn/qq xn/qq yn;-(xn^2+yn^2)/qq 0 zn]; 107 | 108 | %% To find the azimuth and elevation angles of the heliostat normal 109 | 110 | N_azimuth= atan2d(normal(2),normal(1)); 111 | l=sqrt(normal(2)^2+normal(1)^2); 112 | N_elevation = atand(normal(3)/l); % ANGLE OF THE MIRRO NORMAL WRT HORIZONTAL. 113 | %IN THE AZ-EL HELIOSTAT MADE IN THE LAB, THE MOTOR ANGLE 114 | % SHOULD BE 90-N_elevation 115 | 116 | % Motor_angle_azimuth = N_azimuth - 3 ; 117 | % Motor_angle_elevation = 90-N_elevation - 6; 118 | % Motor_angle_azimuth = N_azimuth - 3 ; 119 | % Motor_angle_elevation = 90-N_elevation - 5; 120 | Motor_angle_azimuth = N_azimuth-10 ; 121 | Motor_angle_elevation = 90-N_elevation-18 ; 122 | 123 | 124 | counts_per_degree = 9091; 125 | % 1 degree = 9091 counts of the motor 126 | azi_count = counts_per_degree*Motor_angle_azimuth; 127 | ele_count = counts_per_degree*Motor_angle_elevation; 128 | 129 | Motor_angle_Az_El = cat(1,Motor_angle_Az_El,[Motor_angle_azimuth Motor_angle_elevation]) 130 | Az_El_count = cat(1, Az_El_count, [azi_count -ele_count]) % elevation is multiplied by a -ve sign 131 | % bcoz the Az-El heliostat made 132 | % in the lab rotates in -ve 133 | % direction if a positive value 134 | % is given and vice versa. This 135 | % is bcoz of the direction of 136 | % the axis chosen. 137 | zz=zz+1; 138 | end 139 | 140 | 141 | % Az_El_count = [100000 100000; -100000 -100000] 142 | %% for Galil Motion Control 143 | g = actxserver('galil');%set the variable g to the GalilTools COM wrapper 144 | response = g.libraryVersion;%Retrieve the GalilTools library versions 145 | disp(response);%display GalilTools library version 146 | % g.address = '';%Open connections dialog box 147 | g.address = '192.168.1.1';%Open connections dialog box 148 | 149 | g.command('TPA=0'); 150 | g.command('TPB=0'); 151 | g.command('SH'); 152 | 153 | 154 | [srt_azel_cnt,Ind] = sort(abs(Az_El_count),2); 155 | 156 | for i=1:length(t_1200:t_530); 157 | Enc_cnt = 0.00; 158 | str1 = ['''PAA=',num2str(Az_El_count(i,1)),'''']; 159 | % str2 = ['''PA',num2str(Az_El_count(i,:)),'''']; 160 | str2 = ['''PAB=',num2str(Az_El_count(i,2)),'''']; 161 | g.command(eval(str1)) 162 | g.command(eval(str2)) 163 | g.command('BGA'); 164 | % g.command('AMA'); 165 | g.command('BGB'); 166 | while (abs(Enc_cnt) <=abs(srt_azel_cnt(i,end))) 167 | if Ind(i,end) ==1 168 | Enc_cnt = str2num(g.command('TPA')) 169 | elseif Ind(i,end) == 2 170 | Enc_cnt = str2num(g.command('TPB')) 171 | end 172 | % break; 173 | % pause(1) 174 | end 175 | pause 176 | % enc_A(i)= (str2double(g.command('TPA')))/9091 ; 177 | % enc_B(i)= (str2double(g.command('TPB')))/9091 ; 178 | end 179 | 180 | % g.command('PAA=0'); 181 | % g.command('PAB=0'); 182 | % g.command('BGA'); 183 | % g.command('BGB'); 184 | 185 | % for i=1:length(Az_El_count) 186 | 187 | % str1 = ['''PRA=',num2str(Az_El_count(i,1)),'''']; 188 | % % str2 = ['''PA',num2str(Az_El_count(i,:)),'''']; 189 | % str2 = ['''PRB=',num2str(Az_El_count(i,2)),'''']; 190 | % g.command(eval(str1)) 191 | % g.command(eval(str2)) 192 | % g.command('BGA'); 193 | % while (str2double(Enc_cnt_A) <=Az_El_count(i,1)) 194 | % 195 | % % g.command('AMA'); 196 | % Enc_cnt_A = g.command('TPA') 197 | % break; 198 | % % pause(1) 199 | % end 200 | % tt=tt+1 201 | % % while (str2double(g.command('TPB=?')) <=Az_El_count(i,2)) 202 | % % g.command('BGB'); 203 | % % % g.command('AMB'); 204 | % % pause(1) 205 | % % end 206 | % pause(6) 207 | % 208 | % end 209 | % g.command('ST'); 210 | % g.command('MO'); 211 | % i=1; 212 | 213 | 214 | % % g.command('MO'); 215 | 216 | % figure(2) 217 | % plot(LT(t_900:t_415)/3600,Motor_angle_Az_El(:,2),'r','LineWidth',2) 218 | % hold on 219 | % plot(LT(t_900:t_415)/3600,Motor_angle_Az_El(:,1),'b','LineWidth',2) 220 | % plot(LT(t_900:t_415)/3600,-enc_B,'r*','LineWidth',2) 221 | % plot(LT(t_900:t_415)/3600,enc_A,'b*','LineWidth',2) 222 | % xlabel('Time \it{t / hr}') 223 | % ylabel('Motor angles \it{\theta / (^0)}') 224 | % title('Actuation required for the motor') 225 | % lgnd=legend('Ref. Elevation',' Ref. Azimuth','Encoder Elevation','Encoder Azimuth'); 226 | % grid on 227 | 228 | % g.command('TPB=0') 229 | % 230 | % g.command('SH') 231 | % g.command('PRB=80000') 232 | % g.command('BGB') 233 | % 234 | % g.command('MOB') 235 | % 236 | % enc_counts = [40000 80000 120000 160000 200000 240000 200000 160000 120000 80000 30000 0 30000 80000 30000 110000]'; 237 | % acc = [0.3 9.8; 1.2 9.8; 1.9 9.6; 2.8 9.5; 3.6 9.2; 4.3 8.9; 4.9 8.6; 4.3 8.9; 3.6 9.2; 2.9 9.5; 2.1 9.6; 1.2 9.7; 0.7 9.8;1.2 9.8; 2.2 9.6; 1.3 9.8; 2.8 9.5]; 238 | % 239 | % % aas = [0,0;5.22765254675825,40000;9.44170656622844,80000;14.6687826382438,120000;19.6172174092716,160000;24.0339233227428,200000;27.9196772520063,240000;24.0339233227428,200000;19.6172174092716,160000;15.2220946078582,120000;10.5856824182547,80000;5.29890114776965,30000;0.2,0;5.22765254675825,30000;11.1540038111943,80000;5.80293919636792,30000;14.6687826382438,110000] 240 | % jj = 1; 241 | % theta = []; 242 | % for i=1: length(acc) 243 | % ang(i) = atan2d(acc(i,1),acc(i,2)) ; 244 | % if i>1 245 | % theta = cat(1,theta,ang(i)-ang(1)); 246 | % 247 | % end 248 | % end 249 | % 250 | % plot(theta,enc_counts, 'r*') 251 | % 252 | % 253 | % clc 254 | % ff=fit(theta,enc_counts,'poly1') 255 | % ff.p1 256 | % ff.p2 257 | -------------------------------------------------------------------------------- /src/AzimuthElevation/mainprog_AZ_EL_3_UPU.m: -------------------------------------------------------------------------------- 1 | % This program uses fsolve to solve for AZ-EL case. Since there are only 2 | % two rotations happening, viz. Rz and Ry, in AZ-EL case, the R(3,2) element of the rotation 3 | % matrix remains zero. The sun vector, the reflected ray are completely 4 | % known(since the center is fixed). Hence, the direction cosines of the 5 | % normal can be found out. Therefore there are only 5 unknowns, viz., 6 | % n1,n2,n3,o1 and o2 which are related by 5 equations, viz., n.n=1, o.o=1, 7 | % n.a=0, n.o=0, a.o=0 ; Fsolve is used to solve this. It should be noted 8 | % that depending upon the values of the initial condition, x0, the solution 9 | % changes 10 | clear all 11 | home 12 | close all 13 | global O1M OP Rot_gam OO1 14 | 15 | set(0,'defaulttextinterpreter','latex') 16 | % set(0,'DefaultAxesFontName', 'Times New Roman') 17 | set(0,'DefaultAxesFontSize', 15) 18 | % set(0,'DefaultAxesFontWeight', 'Bold') 19 | 20 | % Change default text fonts. 21 | % set(0,'DefaultTextFontname', 'Times New Roman') 22 | set(0,'DefaultTextFontSize', 15) 23 | 24 | % Bangalore Latitude 25 | lad=12; 26 | lam=58; 27 | las=13; 28 | load('Rot_mat_TA.mat') 29 | % Bangalore Longitude 30 | lod=77; 31 | lom=33; 32 | los=37; 33 | 34 | latitude=lad+(lam+las/60)/60; % in degrees 35 | longitude=lod+(lom+los/60)/60; % in degrees 36 | %% 37 | % Rajasthan Coordinates 38 | latitude = 27.0238; 39 | longitude = 74.2179; 40 | % %% 41 | a=0;b=0;c=65; % Receiver coordinates wrt gcs 42 | OP = [a;b;c]; % Receiver vector 43 | year=2016; 44 | month=6; 45 | day=20; 46 | d=date(year,month,day); 47 | 48 | time_incre=120; % seconds 49 | LT=0*3600:time_incre:24*3600; 50 | 51 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); % aximuth and elevation of sun 52 | [xs, ys, zs]=sunposition(elevation, azimuth); % x2,y2,z2 are coordinates of the sun 53 | OS=[xs;ys;zs]; % Sun vector 54 | rad=100; % radius of the mirror from the origin 55 | psi=30; % psi is the angle the heliostat centre makes with the local east(X) axis 56 | % zG=0.2365; % height of the heliostat 57 | zG=2; 58 | 59 | % KJ = -0.0485; % distance between the mirror center and the top platform center 60 | KJ = -0.25; 61 | 62 | O1M = [0;0;zG]; % vector from the base origin to the mirror origin described wrt bcs 63 | OO1=[rad*cosd(psi);rad*sind(psi);0]; % vector from the global origin to the base origin 64 | v=8/time_incre*3600+1; % start time for the plots 65 | w=17/time_incre*3600+1; % end time for the plots 66 | 67 | gamma = 135; % the angle by which the x_b is inclined towards X (East) 68 | Rot_gam=[ cosd(gamma) -sind(gamma) 0; 69 | sind(gamma) cosd(gamma) 0; 70 | 0 0 1]; 71 | ang = 90; 72 | [Rot,RotZ,eta,Zeta]=Rotmatrix_AzEl(elevation(v:w)',azimuth(v:w)',ang); % Using the normal to find the rotmat 73 | 74 | Rot=RotZ; 75 | for i=1:length(Rot) 76 | O1G(:,i) = O1M+KJ*Rot(:,3,i); 77 | end 78 | %% Leg lengths 79 | figure(1) 80 | rb=0.50; % circum radius of the base equilateral triangle, B1B2/sqrt(3) in meters 81 | rp=0.50; % circum radius of the platform,P1P2/sqrt(3)in meters 82 | 83 | ss=0.08; % square mirror half length 84 | % M is the centre of the mirror and M1, M2, M3, and M4 are the corners 85 | MM1=[ss;ss;0]; MM2=[-ss;ss;0];MM3=[-ss;-ss;0]; MM4=[ss;-ss;0]; 86 | 87 | O1R1=[rb;0;0];O1R2=[-0.5*rb;sqrt(3)*rb/2;0];O1R3=[-0.5*rb;-sqrt(3)*rb/2;0]; 88 | O1Rs=[O1R1 O1R2 O1R3]; 89 | 90 | GS1=[rp;0;0];GS2=[-0.5*rp;sqrt(3)*rp/2;0];GS3=[-0.5*rp;-sqrt(3)*rp/2;0]; 91 | GSs=[GS1 GS2 GS3]; 92 | for i=1:length(Rot) 93 | 94 | O1S1=Rot(:,:,i)*GS1+O1G(:,i); % w.r.t base coordinate system 95 | l1_dir(:,i)=O1S1-O1R1; 96 | l1(i,:)=norm(l1_dir(:,i)); 97 | 98 | O1S2=Rot(:,:,i)*GS2+O1G(:,i); 99 | l2_dir(:,i)=O1S2-O1R2; 100 | l2(i,:)=norm(l2_dir(:,i)); 101 | 102 | O1S3=Rot(:,:,i)*GS3+O1G(:,i); 103 | l3_dir(:,i)=O1S3-O1R3; 104 | l3(i,:)=norm(l3_dir(:,i)); 105 | end 106 | 107 | plot(LT(v:w)/3600, l1*1000,'r','LineWidth', 2) 108 | hold on 109 | plot(LT(v:w)/3600, l2*1000,'g','LineWidth', 2) 110 | plot(LT(v:w)/3600, l3*1000,'b','LineWidth', 2) 111 | xlabel('Time, hrs') 112 | ylabel('leg length, mm') 113 | % title('leg length Vs Time Az-El of 3-UPU') 114 | legend({'leg1','leg2', 'leg3'},'interpreter','latex') 115 | 116 | % leg_vec = [LT(v:w)'/3600 l1*1000 l2*1000 l3*1000]; 117 | % leg_vec(1:40,:) = []; 118 | % load('Rot_mat_TA.mat') 119 | % for i=1:length(Rot) 120 | % Rzz(:,:,i) = Rot(:,:,i)'*Rot_TA(:,:,i); 121 | % angle(i,:) = atan2d(Rzz(2,1,i),Rzz(1,1,i)); 122 | % end 123 | 124 | %% For Animation 125 | TransmatrN=[Rot_gam,OO1;[0 0 0 1]]; % transformation which takes base to global coordinate system 126 | base=TransmatrN*[O1Rs;[1 1 1]]; 127 | % writerObj=VideoWriter('3UPU.avi'); 128 | % writerObj.FrameRate = 9; 129 | % open(writerObj); 130 | OM = OO1 + Rot_gam*O1M; % to mirror centre, M, in gcs 131 | MP = (OP-OM)/norm((OP-OM),2); % wrt gcs reflected ray 132 | 133 | figure(2) 134 | for i=1:length(Rot)% 135 | % for i =1:5 136 | xxx=base(1,:); % wrt global coordinate system 137 | yyy=base(2,:); % wrt global coordinate system 138 | zzz=base(3,:); % wrt global coordinate system 139 | fill3(xxx,yyy,zzz,'r') 140 | hold on 141 | 142 | OM1 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM1); 143 | OM2 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM2); 144 | OM3 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM3); 145 | OM4 = OO1 + Rot_gam*(O1M+Rot(:,:,i)*MM4); 146 | 147 | Mirror=[OM1 OM2 OM3 OM4]; 148 | xm=Mirror(1,:); 149 | ym=Mirror(2,:); 150 | zm=Mirror(3,:); 151 | % fill3(xm,ym,zm,'g') % top platform or mirror 152 | 153 | OS1=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS1); 154 | OS2=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS2); 155 | OS3=OO1+Rot_gam*(O1G(:,i)+Rot(:,:,i)*GS3); 156 | top_triangle=[OS1 OS2 OS3]; 157 | xt=top_triangle(1,:); 158 | yt=top_triangle(2,:); 159 | zt=top_triangle(3,:); 160 | fill3(xt,yt,zt,'c') % top triangle 161 | 162 | TRANSMATR=Rot(:,:,i); 163 | plot3([xt(1) xxx(1)],[yt(1) yyy(1)],[zt(1) zzz(1)],'--k','LineWidth',2) 164 | plot3([xt(2) xxx(2)],[yt(2) yyy(2)],[zt(2) zzz(2)],'--k','LineWidth',2) 165 | plot3([xt(3) xxx(3)],[yt(3) yyy(3)],[zt(3) zzz(3)],'--k','LineWidth',2) 166 | % %% changing wrt global coordinate system 167 | 168 | kons=1;kon=.5;zz=v-1+i; 169 | % normal=plot3([OM(1,1) OM(1,1)+kon*Rot(1,3,i)],[OM(2,1) OM(2,1)+kon*Rot(2,3,i)],[OM(3,1) OM(3,1)+kon*Rot(3,3,i)],'r','LineWidth',2); 170 | % proj=plot3([B0t(1,i) B0t(1,i)+kon*Trans_matr(1,3,i)],[B0t(2,i) B0t(2,i)+kon*Trans_matr(2,3,i)],[0 0],'r','LineWidth',2); 171 | 172 | % sunposition=plot3([OM(1,1) OM(1,1)+kons*xs(zz)],[OM(2,1) OM(2,1)+kons*ys(zz)],[OM(3,1) OM(3,1)+kons*zs(zz)],'k','LineWidth',2); % position of the sun at every time_incre looking from plate coordinte system 173 | plot3(OM(1,1)+kons*xs(v:zz),OM(2,1)+kons*ys(v:zz),OM(3,1)+kons*zs(v:zz),'c*'); % position of the sun at every time_incre looking from plate coordinte system 174 | 175 | %% 176 | kjs =.5; 177 | % plot3([OM(1,1) OM(1,1)+kjs*MP(1,1)],[OM(2,1) OM(2,1)+kjs*MP(2,1)],[OM(3,1) OM(3,1)+kjs*MP(3,1)],'b','LineWidth',2); 178 | 179 | %% 180 | % % coordintes of the receiver 181 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'b','LineWidth',2); 182 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'k*'); 183 | % plot3([a a],[b b],[0 c],'b'); 184 | 185 | % xlim([10 30]); ylim([40 50]); zlim([0 20]); 186 | % xlim([-50 120]); ylim([0 30]); zlim([0 50]); 187 | mg = .7; 188 | axis([OO1(1)-mg OO1(1)+mg OO1(2)-mg OO1(2)+mg 0 2.5]); 189 | % axis([70 71.50 70 72 0 2.5]); 190 | % view(82,6) % to see the 191 | % Receiver 192 | axis square 193 | % axis tight 194 | view(45,45); 195 | grid on 196 | xlabel('X, East','Rotation',-35) 197 | ylabel('Y, North ','Rotation',40) 198 | zlabel('Z, Zenith') 199 | hold off 200 | M(i)=getframe(gcf); 201 | % writeVideo(writerObj,M(i)); 202 | % pause(0.1) 203 | end 204 | % close(writerObj) 205 | 206 | %% Using fsolve to find the rotation matrix for Az-El case 207 | % o3=0; % three-two element of the rotation matrix 208 | 209 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; 210 | % % x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 211 | % % x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 212 | % % x0 = [0.2903; -0.4146; -0.8625; 0.8192; 0.5735]; % for 3rd quadrant 213 | % x0 = [0.2903; -0.4146; 0.5625; -0.8192; 0.5735]; 214 | % % x0 = [0.1761;-0.5876;-0.7897;0.9579;0.2871]; % Initial guesses for n1,n2,n3,o1 and o2 215 | % options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 216 | % for i=1:length(v:w) 217 | % zz=v-1+i; 218 | % fhandle=@(x)for_mainprog_AZ_EL_3_UPU(x,OS(:,zz)); 219 | % [x,fval] = fsolve(fhandle,x0,options); 220 | % x0=x; 221 | % n1=x(1);n2=x(2);n3=x(3);o1=x(4);o2=x(5); 222 | % GS=Rot_gam'*OS(:,zz); % Sun vector in bcs 223 | % GN=(GS+GP)/norm((GS+GP),2); % unit mirror normal wrt bcs 224 | % a1=GN(1); a2=GN(2); a3=GN(3); 225 | % 226 | % Trans_matr(:,:,i)= [n1 o1 a1 O1M(1); % transformation which takes mirror to base coordinate system 227 | % n2 o2 a2 O1M(2); 228 | % n3 o3 a3 O1M(3); 229 | % 0 0 0 1] ; 230 | % Rot(:,:,i)= [n1 o1 a1 ; % rotation which takes mirror to base coordinate system 231 | % n2 o2 a2 ; 232 | % n3 o3 a3]; 233 | % % R(:,:,i) = Rot_gam*Rot(:,:,i); % rotation which takes mirror to global coordinate system 234 | % O1G(:,i) = O1M+KJ*Rot(:,3,i); % Vector from the base origin to the origin of the top platform 235 | % % plot3(O1G(1,i),O1G(2,i),O1G(3,i),'r*') 236 | % % hold on 237 | % end 238 | 239 | %% To check the rotaion matrix 240 | % for i=1:length(eta) 241 | % R1 = [cosd(eta(i)) -sind(eta(i)) 0; 242 | % sind(eta(i)) cosd(eta(i)) 0; 243 | % 0 0 1]; 244 | % R2 = [cosd(90-Zeta(i)) 0 sind(90-Zeta(i)); 245 | % 0 1 0 ; 246 | % -sind(90-Zeta(i)) 0 cosd(90-Zeta(i))]; 247 | % Rt(:,:,i) = R1*R2; 248 | % end -------------------------------------------------------------------------------- /src/expt/az-el/Az_El.m: -------------------------------------------------------------------------------- 1 | %inputs needed are station co-ordinates longitude,latitude in degrees,min,sec ... 2 | %altitude in kms and time of proposed sun visibility in year,month,day in Christian era, 3 | %hr,min and sec in GMT (NOT BASED ON LOCAL TIME). 4 | %Input the following 5 | clear all 6 | home 7 | close all 8 | clc 9 | %% The following part of the code is used to mark point on the lab wall to show sun vector 10 | % Bangalore Latitude 11 | lad=12; 12 | lam=58; 13 | las=13; 14 | 15 | % Bangalore Longitude 16 | lod=77; 17 | lom=33; 18 | los=37; 19 | 20 | latitude=lad+(lam+las/60)/60; % in degrees 21 | longitude=lod+(lom+los/60)/60; % in degrees 22 | 23 | year=2013; 24 | month=3; 25 | day=21; 26 | d=date(year,month,day); 27 | 28 | time_incre=900; % in seconds or in increments of 15 minutes 29 | LT=0:time_incre:24*3600; 30 | 31 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 32 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 33 | OS=[xs;ys;zs]; 34 | 35 | 36 | % The lab wall is in the -ve x direction. So chosing a sun-vector in the 37 | % evening, say, 3:15pm, to 4:15. Also, the portion of 38 | % lab wall corresponding to the above time steps is neat and clean. 39 | t_900 = 9/time_incre*3600+1; 40 | t_915 = t_900 + 1; 41 | t_930 = t_900 + 2; 42 | t_945 = t_900 + 3; 43 | t_1000 = 10/time_incre*3600+1; 44 | t_1015 = t_1000 + 1; 45 | t_1030 = t_1000 + 2; 46 | t_1045 = t_1000 + 3; 47 | 48 | t_1100 = 11/time_incre*3600+1; 49 | t_1115 = t_1100 + 1; 50 | t_1130 = t_1100 + 2; 51 | t_1145 = t_1100 + 3; 52 | 53 | t_1200 = 12/time_incre*3600+1; 54 | t_1215 = t_1200 + 1; 55 | t_1230 = t_1200 + 2; 56 | t_1245 = t_1200 + 3; 57 | 58 | t_1300 = 13/time_incre*3600+1; 59 | t_1315 = t_1300 + 1; 60 | t_1330 = t_1300 + 2; 61 | t_1345 = t_1300 + 3; 62 | 63 | t_200 = 14/time_incre*3600+1; 64 | t_215 = t_200 + 1; 65 | t_230 = t_200 + 2; 66 | t_245 = t_200 + 3; 67 | t_300 = t_200 + 4; 68 | t_315 = t_200 + 5; 69 | t_330 = t_200 + 6; 70 | t_345 = t_200 + 7; 71 | t_400 = 16/time_incre*3600+1; 72 | t_415 = t_400 + 1; 73 | t_430 = t_400 + 2; 74 | t_445 = t_400 + 3; 75 | t_500 = 17/time_incre*3600+1; 76 | t_515 = t_500 + 1; 77 | t_530 = t_500 + 2; 78 | %% Plot of azimuth and elevation angles 79 | figure(1) 80 | plot(LT(t_900:t_415)/3600,elevation((t_900:t_415)),'r','LineWidth',4) 81 | hold on 82 | plot(LT(t_900:t_415)/3600,azimuth((t_900:t_415)),'b','LineWidth',4) 83 | xlabel('Time \it{t / hr}') 84 | ylabel('Sun vector angles \it{\theta / (^0)}') 85 | title('Azimuth and Elevation angles of the sun') 86 | lgnd=legend('Elevation',' Azimuth'); 87 | grid on 88 | % 37-67 % 9am - 4.30pm 89 | 90 | % To mark on the lab roof which is at 4.2m from the chosen global 91 | % origin, the OS vector has to be multiplied by a constant so as to get the 92 | % z as 4.2m 93 | v_900 = OS(:,t_900)*(4.20/OS(3,t_900)); 94 | v_1000 = OS(:,t_1000)*(4.20/OS(3,t_1000)); 95 | v_1100 = OS(:,t_1100)*(4.20/OS(3,t_1100)); 96 | v_1200 = OS(:,t_1200)*(4.20/OS(3,t_1200)); 97 | v_1300 = OS(:,t_1300)*(4.20/OS(3,t_1300)); 98 | 99 | v_200 = OS(:,t_200)*(4.20/OS(3,t_200)); 100 | v_215 = OS(:,t_215)*(4.20/OS(3,t_215)); 101 | v_230 = OS(:,t_230)*(4.20/OS(3,t_230)); 102 | v_245 = OS(:,t_245)*(4.20/OS(3,t_245)); 103 | v_300 = OS(:,t_300)*(4.20/OS(3,t_300)); 104 | 105 | % To mark on the lab wall which is at -3.545m from the chosen global 106 | % origin, the OS vector has to be multiplied by a constant so as to get the 107 | % x as -3.545m 108 | v_315 = OS(:,t_315)*(-3.540/OS(1,t_315)); 109 | v_330 = OS(:,t_330)*(-3.540/OS(1,t_330)); 110 | v_345 = OS(:,t_345)*(-3.540/OS(1,t_345)); 111 | v_400 = OS(:,t_400)*(-3.540/OS(1,t_400)); 112 | v_415 = OS(:,t_415)*(-3.540/OS(1,t_415)); 113 | v_430 = OS(:,t_430)*(-3.540 /OS(1,t_430)); 114 | 115 | % Tgt =[v_900 v_1000 v_1100 v_1200 v_1300 v_215 v_230 v_245 v_300 v_315 v_330 v_345 v_400 v_415] 116 | 117 | %% 118 | Motor_angle_Az_El =[]; Az_El_count =[]; 119 | % xs = 5.55;ys= -1.336; zs = 2.62; % LASER coordinates 120 | xl = 6.38; yl = -1.21; zl = 2.56; % laser coordinates mounted on the wall 121 | zG = 1.58; 122 | B0t = [0;0;zG]; % Heliostat centre. 123 | zz = t_900; 124 | 125 | for i=1:length(t_900:t_530) 126 | GP = OS(:,zz); 127 | % OP = OS(:,66); 128 | % GP=(OP-B0t)/norm((OP-B0t),2); 129 | 130 | % GP = OS(:,66); 131 | OL=[xl;yl;zl]; % laser vector 132 | GS=(OL-B0t)/norm((OL-B0t),2); 133 | n = GS + GP; 134 | normal=n/norm(n,2); 135 | xn=normal(1); yn=normal(2); zn=normal(3); qq=sqrt(xn^2+yn^2); 136 | rotmat =[xn*zn/qq -yn/qq xn ;yn*zn/qq xn/qq yn;-(xn^2+yn^2)/qq 0 zn]; 137 | 138 | %% To find the azimuth and elevation angles of the heliostat normal 139 | 140 | N_azimuth= atan2d(normal(2),normal(1)); 141 | l=sqrt(normal(2)^2+normal(1)^2); 142 | N_elevation = atand(normal(3)/l); % ANGLE OF THE MIRRO NORMAL WRT HORIZONTAL. 143 | %IN THE AZ-EL HELIOSTAT MADE IN THE LAB, THE MOTOR ANGLE 144 | % SHOULD BE 90-N_elevation 145 | 146 | % Motor_angle_azimuth = N_azimuth - 3 ; 147 | % Motor_angle_elevation = 90-N_elevation - 6; 148 | % Motor_angle_azimuth = N_azimuth - 3 ; 149 | % Motor_angle_elevation = 90-N_elevation - 5; 150 | Motor_angle_azimuth = N_azimuth ; 151 | Motor_angle_elevation = 90-N_elevation ; 152 | 153 | 154 | counts_per_degree = 9091; 155 | % 1 degree = 9091 counts of the motor 156 | azi_count = counts_per_degree*Motor_angle_azimuth; 157 | ele_count = counts_per_degree*Motor_angle_elevation; 158 | 159 | Motor_angle_Az_El = cat(1,Motor_angle_Az_El,[Motor_angle_azimuth Motor_angle_elevation]) 160 | Az_El_count = cat(1, Az_El_count, [azi_count -ele_count]) % elevation is multiplied by a -ve sign 161 | % bcoz the Az-El heliostat made 162 | % in the lab rotates in -ve 163 | % direction if a positive value 164 | % is given and vice versa. This 165 | % is bcoz of the direction of 166 | % the axis chosen. 167 | zz=zz+1; 168 | end 169 | 170 | 171 | % Az_El_count = [100000 100000; -100000 -100000] 172 | %% for Galil Motion Control 173 | g = actxserver('galil');%set the variable g to the GalilTools COM wrapper 174 | response = g.libraryVersion;%Retrieve the GalilTools library versions 175 | disp(response);%display GalilTools library version 176 | % g.address = '';%Open connections dialog box 177 | g.address = '192.168.1.1';%Open connections dialog box 178 | 179 | g.command('TPA=0'); 180 | g.command('TPB=0'); 181 | g.command('SH'); 182 | 183 | 184 | [srt_azel_cnt,Ind] = sort(abs(Az_El_count),2); 185 | 186 | for i=1:length(t_900:t_530); 187 | Enc_cnt = 0.00; 188 | str1 = ['''PAA=',num2str(Az_El_count(i,1)),'''']; 189 | % str2 = ['''PA',num2str(Az_El_count(i,:)),'''']; 190 | str2 = ['''PAB=',num2str(Az_El_count(i,2)),'''']; 191 | g.command(eval(str1)) 192 | g.command(eval(str2)) 193 | g.command('BGA'); 194 | % g.command('AMA'); 195 | g.command('BGB'); 196 | while (abs(Enc_cnt) <=abs(srt_azel_cnt(i,end))) 197 | if Ind(i,end) ==1 198 | Enc_cnt = str2num(g.command('TPA')) 199 | elseif Ind(i,end) == 2 200 | Enc_cnt = str2num(g.command('TPB')) 201 | end 202 | % break; 203 | % pause(1) 204 | end 205 | pause(5) 206 | % enc_A(i)= (str2double(g.command('TPA')))/9091 ; 207 | % enc_B(i)= (str2double(g.command('TPB')))/9091 ; 208 | end 209 | 210 | g.command('PAA=0'); 211 | g.command('PAB=0'); 212 | g.command('BGA'); 213 | g.command('BGB'); 214 | 215 | % for i=1:length(Az_El_count) 216 | 217 | % str1 = ['''PRA=',num2str(Az_El_count(i,1)),'''']; 218 | % % str2 = ['''PA',num2str(Az_El_count(i,:)),'''']; 219 | % str2 = ['''PRB=',num2str(Az_El_count(i,2)),'''']; 220 | % g.command(eval(str1)) 221 | % g.command(eval(str2)) 222 | % g.command('BGA'); 223 | % while (str2double(Enc_cnt_A) <=Az_El_count(i,1)) 224 | % 225 | % % g.command('AMA'); 226 | % Enc_cnt_A = g.command('TPA') 227 | % break; 228 | % % pause(1) 229 | % end 230 | % tt=tt+1 231 | % % while (str2double(g.command('TPB=?')) <=Az_El_count(i,2)) 232 | % % g.command('BGB'); 233 | % % % g.command('AMB'); 234 | % % pause(1) 235 | % % end 236 | % pause(6) 237 | % 238 | % end 239 | % g.command('ST'); 240 | % g.command('MO'); 241 | % i=1; 242 | 243 | 244 | % % g.command('MO'); 245 | 246 | figure(2) 247 | plot(LT(t_900:t_415)/3600,Motor_angle_Az_El(:,2),'r','LineWidth',2) 248 | hold on 249 | plot(LT(t_900:t_415)/3600,Motor_angle_Az_El(:,1),'b','LineWidth',2) 250 | plot(LT(t_900:t_415)/3600,-enc_B,'r*','LineWidth',2) 251 | plot(LT(t_900:t_415)/3600,enc_A,'b*','LineWidth',2) 252 | xlabel('Time \it{t / hr}') 253 | ylabel('Motor angles \it{\theta / (^0)}') 254 | title('Actuation required for the motor') 255 | lgnd=legend('Ref. Elevation',' Ref. Azimuth','Encoder Elevation','Encoder Azimuth'); 256 | grid on 257 | 258 | g.command('TPB=0') 259 | 260 | g.command('SH') 261 | g.command('PRB=80000') 262 | g.command('BGB') 263 | 264 | g.command('MOB') 265 | 266 | enc_counts = [40000 80000 120000 160000 200000 240000 200000 160000 120000 80000 30000 0 30000 80000 30000 110000]'; 267 | acc = [0.3 9.8; 1.2 9.8; 1.9 9.6; 2.8 9.5; 3.6 9.2; 4.3 8.9; 4.9 8.6; 4.3 8.9; 3.6 9.2; 2.9 9.5; 2.1 9.6; 1.2 9.7; 0.7 9.8;1.2 9.8; 2.2 9.6; 1.3 9.8; 2.8 9.5]; 268 | 269 | % aas = [0,0;5.22765254675825,40000;9.44170656622844,80000;14.6687826382438,120000;19.6172174092716,160000;24.0339233227428,200000;27.9196772520063,240000;24.0339233227428,200000;19.6172174092716,160000;15.2220946078582,120000;10.5856824182547,80000;5.29890114776965,30000;0.2,0;5.22765254675825,30000;11.1540038111943,80000;5.80293919636792,30000;14.6687826382438,110000] 270 | jj = 1; 271 | theta = []; 272 | for i=1: length(acc) 273 | ang(i) = atan2d(acc(i,1),acc(i,2)) ; 274 | if i>1 275 | theta = cat(1,theta,ang(i)-ang(1)); 276 | 277 | end 278 | end 279 | 280 | plot(theta,enc_counts, 'r*') 281 | 282 | 283 | clc 284 | ff=fit(theta,enc_counts,'poly1') 285 | ff.p1 286 | ff.p2 287 | -------------------------------------------------------------------------------- /src/TargetAligned/TA_NEW.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | home 4 | set(0,'defaulttextinterpreter','latex') 5 | % set(0,'DefaultAxesFontName', 'Times New Roman') 6 | set(0,'DefaultAxesFontSize', 15) 7 | % set(0,'DefaultAxesFontWeight', 'Bold') 8 | 9 | % Change default text fonts. 10 | % set(0,'DefaultTextFontname', 'Times New Roman') 11 | set(0,'DefaultTextFontSize', 15) 12 | % X axis-local east, Y axis - local north, Z axis - zenith (global coordinate system) 13 | 14 | % Bangalore Latitude 15 | lad=12; 16 | lam=58; 17 | las=13; 18 | % Bangalore Longitude 19 | lod=77; 20 | lom=33; 21 | los=37; 22 | 23 | latitude=lad+(lam+las/60)/60; % in degrees 24 | longitude=lod+(lom+los/60)/60; % in degrees 25 | 26 | year=2016; 27 | month=3; 28 | day=20; 29 | d=date(year,month,day); 30 | 31 | time_incre=120; % seconds 32 | LT=0*3600:time_incre:24*3600; 33 | 34 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); % elevation and azimuth angles of sun 35 | [xs ,ys, zs]=sunposition(elevation, azimuth); % direction cosines of sun in the sky or sun vector 36 | OS = [xs;ys;zs] ; % Sun vector 37 | a=0; b=0 ; c=65; % Receiver coordinates 38 | OP = [a;b;c]; % Receiver vector from gcs origin, O 39 | 40 | rad=100; % radius of the mirror from the origin 41 | psi=30; % psi is the angle the heliostat centre makes with the local east(X) axis 42 | % zG=0.2365; % height of the heliostat 43 | zG = 2; 44 | 45 | % KJ = -0.0485; % distance between the mirror center, M, and the top platform center, G 46 | KJ = -0.25; 47 | O1M = [0;0;zG]; % vector from the base origin to the mirror origin described wrt bcs 48 | OO1 = [rad*cosd(psi);rad*sind(psi);0]; % vector from the global origin to the base origin wrt gcs 49 | 50 | lambda = atand(rad/(c-zG)); % angle the reflected ray makes with the vertical 51 | gamma = 0; % the angle by which the x_b is inclined towards X (East) 52 | Rot_gam=[ cosd(gamma) -sind(gamma) 0; 53 | sind(gamma) cosd(gamma) 0; % rotation of base CS about z axis 54 | 0 0 1]; 55 | alpha = psi-gamma; 56 | R1 = [cosd(alpha) -sind(alpha) 0; 57 | sind(alpha) cosd(alpha) 0; 58 | 0 0 1]; 59 | R2 = [cosd(-lambda) 0 sind(-lambda); 60 | 0 1 0 ; 61 | -sind(-lambda) 0 cosd(-lambda)] ; 62 | 63 | R11 = R1*R2; 64 | xx=R11(:,1); % direction perpendicular to reflected ray initially 65 | 66 | % P is the receiver / target point 67 | O1P_bcs=Rot_gam'*(OP-OO1); % O1P_bcs = O1P described in bcs (base coordinate system) 68 | g=O1P_bcs-O1M; 69 | GP=g/norm(g,2); % unit reflected ray wrt bcs 70 | 71 | GP1 = (OP-OO1-Rot_gam*O1M)/norm((OP-OO1-Rot_gam*O1M),2); % unit reflected ray in global CS 72 | v=8/time_incre*3600+1; % start time for the plots 73 | w=9/time_incre*3600+1; % end time for the plots 74 | 75 | jl = 200; % switch time 76 | % ang = [zeros(1,jl),-10:-10:-10*(length(v:w)-jl)]; 77 | ang = -60; 78 | 79 | for i=1:length(v:w) 80 | zz=v-1+i; 81 | %% for test 82 | % GS=OS(:,zz); % Sun vector in global CS 83 | % GN(:,i)=(GS+GP1)/norm((GS+GP1),2); % unit normal in global CS 84 | %% 85 | 86 | theta(i)=0.5*(acosd(dot(Rot_gam'*OS(:,zz),GP/norm(GP,2)))); % elevation angle 87 | X=dot(xx,Rot_gam'*OS(:,zz)); 88 | Y=dot(cross(xx,Rot_gam'*OS(:,zz)),GP/norm(GP,2)); 89 | rho(i)=atan2d(Y,X); % spinning angle 90 | 91 | R3 = [cosd(rho(i)) -sind(rho(i)) 0; 92 | sind(rho(i)) cosd(rho(i)) 0; 93 | 0 0 1]; 94 | R4 = [cosd(theta(i)) 0 sind(theta(i)); 95 | 0 1 0 ; 96 | -sind(theta(i)) 0 cosd(theta(i))] ; 97 | Rot_TA(:,:,i) = R11 * R3 * R4; % Rotation matrix at every time instant %% 98 | 99 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 100 | normal = [Rot_TA(1,3,i); Rot_TA(2,3,i); Rot_TA(3,3,i)]; 101 | 102 | Skew_normal_matrix = [ 0 -normal(3) normal(2); 103 | normal(3) 0 -normal(1); 104 | -normal(2) normal(1) 0 ]; 105 | 106 | Rz(:,:,i) = expm((ang*pi/180)*Skew_normal_matrix); 107 | Ak = R1*R2; 108 | Rot_TA(:,:,i) = Rz(:,:,i)*Rot_TA(:,:,i); 109 | % Rot_TA(:,:,jl:end) = Rot_TAZ(:,:,jl:end); 110 | % Rot_TA(:,:,jl:end) = permute(Rot_TA(:,:,jl:end),[2,1,3]); 111 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 112 | 113 | % Trans_matr(:,:,i) = [Rot_TA(:,:,i) O1M]; %% transformation which takes mirror to bcs 114 | O1G(:,i) = O1M+KJ*Rot_TA(:,3,i); %% wrt bcs 115 | end 116 | %% Leg lengths 117 | figure(1) 118 | rb=0.50; % circum radius of the base equilateral triangle 119 | rp=0.50; % circum radius of the platform 120 | 121 | % ss=0.08; 122 | % MM1=[ss;ss;0]; MM2=[-ss;ss;0];MM3=[-ss;-ss;0]; MM4=[ss;-ss;0]; 123 | 124 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 125 | O1Rs=[O1R1 O1R2 O1R3]; 126 | 127 | GS1=[rp;0;0];GS2=[-.5*rp;sqrt(3)*rp/2;0];GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 128 | GSs=[GS1 GS2 GS3]; 129 | 130 | for i=1:length(Rot_TA) 131 | 132 | O1S1 = Rot_TA(:,:,i)*GS1+O1G(:,i); % w.r.t base coordinate system 133 | l1_dir(:,i)= O1S1-O1R1; % direction of leg-length 134 | % l1_dir(:,i)= O1U1-O1R3; 135 | l1(:,i) = norm(l1_dir(:,i)); % leg length 136 | 137 | O1S2=Rot_TA(:,:,i)*GS2+O1G(:,i); 138 | l2_dir(:,i)=O1S2-O1R2; 139 | l2(:,i)=norm(l2_dir(:,i)); 140 | 141 | O1S3=Rot_TA(:,:,i)*GS3+O1G(:,i); 142 | l3_dir(:,i)=O1S3-O1R3; 143 | % l3_dir(:,i)=O1U3-O1R1; 144 | l3(:,i)=norm(l3_dir(:,i)); 145 | end 146 | plot(LT(v:w)/3600, l1*1000,'r','LineWidth', 2) 147 | hold on 148 | plot(LT(v:w)/3600, l2*1000,'g','LineWidth', 2) 149 | plot(LT(v:w)/3600, l3*1000,'b','LineWidth', 2) 150 | xlabel('Time, hrs') 151 | ylabel(' leg length, mm') 152 | % title('leg length Vs Time for T-A of 3-UPU') 153 | legend({'leg1','leg2', 'leg3'},'interpreter','latex') 154 | 155 | %% elevation and spinning angle plots 156 | figure(2) 157 | plot(LT(v:w)/3600 , theta, 'r','LineWidth', 2) 158 | hold on 159 | plot(LT(v:w)/3600 , rho, 'b','LineWidth', 2) 160 | xlabel('Time, hrs') 161 | ylabel('Angle, $^0$') 162 | legend ('angle of incidence / elevation' , 'spinning angle') 163 | 164 | %% For Animation 165 | % FigHandle = figure(3); 166 | % set(FigHandle, 'Position', [100, 100, 1049, 895]); 167 | TransmatrN=[Rot_gam,OO1;[0 0 0 1]]; % transformation which takes base to global coordinate system 168 | base=TransmatrN*[O1Rs;[1 1 1]]; 169 | % writerObj=VideoWriter('TA.mp4'); 170 | % writerObj.FrameRate = 12; 171 | % open(writerObj); 172 | m = 0.5; 173 | MM1= [m;m;0]; MM2= [-m;m;0]; MM3= [-m;-m;0]; MM4= [m;-m;0]; % M is the centre of the mirror. 174 | 175 | for i=1:length(Rot_TA)% 176 | % for i=1:4 177 | xxx=base(1,:); % wrt global coordinate system 178 | yyy=base(2,:); % wrt global coordinate system 179 | zzz=base(3,:); % wrt global coordinate system 180 | fill3(xxx,yyy,zzz,'r') 181 | hold on 182 | OM = OO1 + Rot_gam*O1M; 183 | Om1 = OO1 + Rot_gam*(O1M+Rot_TA(:,:,i)*MM1); 184 | Om2 = OO1 + Rot_gam*(O1M+Rot_TA(:,:,i)*MM2); 185 | Om3 = OO1 + Rot_gam*(O1M+Rot_TA(:,:,i)*MM3); 186 | Om4 = OO1 + Rot_gam*(O1M+Rot_TA(:,:,i)*MM4); 187 | 188 | % Mirror=[OM1 OM2 OM3 OM4]; 189 | % xm=Mirror(1,:); 190 | % ym=Mirror(2,:); 191 | % zm=Mirror(3,:); 192 | % fill3(xm,ym,zm,'g') % top platform or mirror 193 | % Plot of mirror 194 | % plot3([Om1(1) Om2(1)] ,[Om1(2) Om2(2)], [Om1(3) Om2(3)], 'r','LineWidth',2) 195 | % plot3([Om2(1) Om3(1)] ,[Om2(2) Om3(2)], [Om2(3) Om3(3)], 'r','LineWidth',2) 196 | % plot3([Om3(1) Om4(1)] ,[Om3(2) Om4(2)], [Om3(3) Om4(3)], 'r','LineWidth',2) 197 | % plot3([Om4(1) Om1(1)] ,[Om4(2) Om1(2)], [Om4(3) Om1(3)], 'r','LineWidth',2) 198 | 199 | OS1=OO1+Rot_gam*(O1G(:,i)+Rot_TA(:,:,i)*GS1); 200 | OS2=OO1+Rot_gam*(O1G(:,i)+Rot_TA(:,:,i)*GS2); 201 | OS3=OO1+Rot_gam*(O1G(:,i)+Rot_TA(:,:,i)*GS3); 202 | top_triangle=[OS1 OS2 OS3]; 203 | xt=top_triangle(1,:); 204 | yt=top_triangle(2,:); 205 | zt=top_triangle(3,:); 206 | fill3(xt,yt,zt,'c') % top triangle 207 | 208 | plot3([xt(1) xxx(1)],[yt(1) yyy(1)],[zt(1) zzz(1)],'--k','LineWidth',2) 209 | plot3([xt(2) xxx(2)],[yt(2) yyy(2)],[zt(2) zzz(2)],'--k','LineWidth',2) 210 | plot3([xt(3) xxx(3)],[yt(3) yyy(3)],[zt(3) zzz(3)],'--k','LineWidth',2) 211 | % plot3([xt(1) xxx(3)],[yt(1) yyy(3)],[zt(1) zzz(3)],'--k','LineWidth',2.5) 212 | % plot3([xt(2) xxx(2)],[yt(2) yyy(2)],[zt(2) zzz(2)],'--k','LineWidth',2.5) 213 | % plot3([xt(3) xxx(1)],[yt(3) yyy(1)],[zt(3) zzz(1)],'--k','LineWidth',2.5) 214 | 215 | kons=1;kon=.5;zz=v-1+i; 216 | % normal=plot3([OM(1,1) OM(1,1)+kon*Rot_TA(1,3,i)],[OM(2,1) OM(2,1)+kon*Rot_TA(2,3,i)],[OM(3,1) OM(3,1)+kon*Rot_TA(3,3,i)],'r','LineWidth',2); 217 | 218 | % sunposition=plot3([OM(1,1) OM(1,1)+kons*xs(zz)],[OM(2,1) OM(2,1)+kons*ys(zz)],[OM(3,1) OM(3,1)+kons*zs(zz)],'k','LineWidth',2); % position of the sun at every time_incre looking from plate coordinte system 219 | % plot3(OM(1,1)+kons*xs(v:zz),OM(2,1)+kons*ys(v:zz),OM(3,1)+kons*zs(v:zz),'k*'); % position of the sun at every time_incre looking from plate coordinte system 220 | % % % coordintes of the receiver 221 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'b','LineWidth',2); 222 | % plot3([OM(1,1) a],[OM(2,1) b],[OM(3,1) c],'k*'); 223 | % plot3([a a],[b b],[0 c],'b'); 224 | kjs =.5; 225 | % plot3([OM(1,1) OM(1,1)+kjs*GP(1,1)],[OM(2,1) OM(2,1)+kjs*GP(2,1)],[OM(3,1) OM(3,1)+kjs*GP(3,1)],'b','LineWidth',2); 226 | % axis([tx-3 tx+3 ty-3 ty+3 0 1.5]); 227 | 228 | % xlim([10 30]); ylim([40 50]); zlim([0 20]); 229 | % xlim([-50 120]); ylim([0 30]); zlim([0 50]); 230 | % axis([-10 10 -5 5 60 70]); view(82,6) % to see the 231 | % Receiver 232 | 233 | % axis equal 234 | mg = 1; 235 | axis([OO1(1)-mg OO1(1)+mg OO1(2)-mg OO1(2)+mg 0 3.5]); 236 | % axis([tx-1 tx+1 ty-1 ty+1 0 tz+3]); view(24,20) % to see the mirror 237 | % view(-16,18) % to see the mirror 238 | axis square 239 | view(226,28); 240 | grid on 241 | xlabel('X, East','Rotation',-3) 242 | ylabel('Y, North ','Rotation',75) 243 | zlabel('Z, Zenith') 244 | hold off 245 | M(i)=getframe(gcf); 246 | % writeVideo(writerObj,M(i)); 247 | % pause(0.1) 248 | end 249 | % close(writerObj) 250 | 251 | 252 | 253 | 254 | 255 | 256 | -------------------------------------------------------------------------------- /src/expt/3-rps/RPS_lab_FINAL.m: -------------------------------------------------------------------------------- 1 | % this solves 5 equations in 5 unknowns 2 | %% VERY IMP: for some months like june, and some other months like december azimuth = 360-a(1). otherwise time vs az-El graph will not be continuous 3 | % As per the data sheet of chinese linear actuator( HF-TGE), the retracted length (when stroke is 4 | % 0) is 1250mm. So the leg length should not be lower than this value. 5 | % From 1250mm, it can go to a max distance of 2250mm (1000 mm stroke). So 6 | % the maximum of each of the three legs should not exceed 2250 mm. To 7 | % obtain stow position, all the three legs to have equal length. 8 | 9 | close all 10 | clear all 11 | clc 12 | home 13 | global OP t zc Rot rp 14 | 15 | %% 16 | %% The following part of the code is used to mark point on the lab wall to show sun vector 17 | % Bangalore Latitude 18 | lad=12; 19 | lam=58; 20 | las=13; 21 | 22 | % Bangalore Longitude 23 | lod=77; 24 | lom=33; 25 | los=37; 26 | 27 | latitude=lad+(lam+las/60)/60; % in degrees 28 | longitude=lod+(lom+los/60)/60; % in degrees 29 | 30 | year=2016; 31 | month=3; 32 | day=21; 33 | d=date(year,month,day); 34 | 35 | time_incre=900; % in seconds or in increments of 15 minutes 36 | LT=0:time_incre:24*3600; 37 | 38 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 39 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 40 | OS=[xs;ys;zs]; 41 | 42 | % The lab wall is in the -ve x direction. So chosing a sun-vector in the 43 | % evening, say, 3:15pm, 3:30, 3:45, 4:00 pm, 4:15, 4:30. Also, the portion of 44 | % lab wall corresponding to the above time steps is neat and clean. 45 | 46 | t_700 = 7/time_incre*3600+1; 47 | t_800 = 8/time_incre*3600+1; 48 | t_900 = 9/time_incre*3600+1; 49 | t_915 = t_900 + 1; 50 | t_930 = t_900 + 2; 51 | t_945 = t_900 + 3; 52 | 53 | t_1000 = 10/time_incre*3600+1; 54 | t_1015 = t_1000 + 1; 55 | t_1030 = t_1000 + 2; 56 | t_1045 = t_1000 + 3; 57 | 58 | t_1100 = 11/time_incre*3600+1; 59 | t_1115 = t_1100 + 1; 60 | t_1130 = t_1100 + 2; 61 | t_1145 = t_1100 + 3; 62 | 63 | t_1200 = 12/time_incre*3600+1; 64 | t_1215 = t_1200 + 1; 65 | t_1230 = t_1200 + 2; 66 | t_1245 = t_1200 + 3; 67 | 68 | t_100 = 13/time_incre*3600+1; 69 | t_115 = t_100 + 1; 70 | t_130 = t_100 + 2; 71 | t_145 = t_100 + 3; 72 | 73 | t_200 = 14/time_incre*3600+1; 74 | t_215 = t_200 + 1; 75 | t_230 = t_200 + 2; 76 | t_245 = t_200 + 3; 77 | t_300 = t_200 + 4; 78 | t_315 = t_200 + 5; 79 | t_330 = t_200 + 6; 80 | t_345 = t_200 + 7; 81 | 82 | t_400 = 16/time_incre*3600+1; 83 | t_500 = 17/time_incre*3600+1; 84 | t_600 = 18/time_incre*3600+1; 85 | 86 | 87 | % To mark on the lab wall which is at -3.54m from the chosen global 88 | % origin (same as base origin of Az-El), the OS vector has to be multiplied by a constant so as to get the 89 | % x as -6.35m 90 | wall_dist = -3.54; 91 | 92 | v_315 = OS(:,t_315)*(wall_dist/OS(1,t_315)); 93 | v_330 = OS(:,t_330)*(wall_dist/OS(1,t_330)); 94 | v_345 = OS(:,t_345)*(wall_dist/OS(1,t_345)); 95 | v_400 = OS(:,t_400)*(wall_dist/OS(1,t_400)); 96 | % v_415 = OS(:,t_415)*(wall_dist/OS(1,t_415)); 97 | % v_430 = OS(:,t_430)*(wall_dist/OS(1,t_430)); 98 | 99 | % [v_315 v_330 v_345 v_400 v_415 v_430] 100 | %% 101 | 102 | % The base of the Az-El heliostat is the origin of the global co-ordinate 103 | % system. 104 | rb=0.400; 105 | rp=0.250; 106 | % a= -3.545; b=2.12; c=3.1; % receiver co-ordinates 107 | % xs = 6.385;ys= -1.51; zs = 2.56; % LASER co-ordinates one fixed on the window frame 108 | zc=1.64; 109 | tx = 10*.3048; ty =0.0; tz=0; t=[tx;ty;tz]; 110 | % xl = 5.63; yl= -1.22; zl = 2.625; % LASER coordinates wrt Az-El Origin 111 | % xl_3rps = xl-tx; yl_3rps = yl-ty; zl_3rps = zl-zc; 112 | xl = 6.38; yl = -1.21; zl = 2.56; % laser coordinates mounted on the wall 113 | 114 | 115 | % OP=[a;b;c]; 116 | % OP = v_315; 117 | % [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are co-ordinates of the sun 118 | 119 | % OS1=[xl_3rps;yl_3rps;zl_3rps]; 120 | % unit_sun = OS1/norm(OS1,2); 121 | 122 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 123 | O1Rs=[O1R1 O1R2 O1R3]; 124 | 125 | GS1=[rp;0;0]; GS2=[-.5*rp;sqrt(3)*rp/2;0]; GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 126 | GSs=[GS1 GS2 GS3]; 127 | 128 | gamma=0; % the angle x_b(base coordinate system) makes with local-east 129 | Rot=[ cosd(gamma) -sind(gamma) 0; 130 | sind(gamma) cosd(gamma) 0; 131 | 0 0 1]; 132 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; 133 | % x0 = [.7;-.6;0.1;-0.7;-0.01;-0.05]; 134 | x0 = [-.7;-.6;-0.1;-0.7;0.01;-0.05]; 135 | % x0=[0.25; 0.6; 0.4; 0.35; 0.56]; 136 | % x0 = [1.7;.6;-1;.7;.1;.5;0.08;1]; 137 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 138 | % options = optimoptions('fsolve','Display','iter','TolX',1e-9); 139 | % options = optimoptions('fsolve','Jacobian','off'); 140 | 141 | %% 142 | % OS_d = [OS(:,t_700) OS(:,t_800) OS(:,t_900) OS(:,t_1000) OS(:,t_1100) OS(:,t_1200) OS(:,t_100) OS(:,t_200) OS(:,t_300) OS(:,t_400) OS(:,t_500)];% OS(:,t_600)]; 143 | OS_d = [OS(:,t_1000) OS(:,t_1100) OS(:,t_1200) OS(:,t_100) OS(:,t_200) OS(:,t_300) OS(:,t_400) OS(:,t_500)];% OS(:,t_600)]; 144 | 145 | Act_req = []; 146 | % tme = [7 8 9 10 11 12 1 2 3 4 5]; 147 | tme = [10 11 12 1 2 3 4 5]; 148 | l10 = 1.645; l20 = 1.653; l30 = 1.650; 149 | OP = [xl;yl;zl]; 150 | for i=1:length(OS_d) 151 | fhandle=@(x)for_mainprog_3RPS(x,OS_d(:,i)); 152 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 153 | x0=xval(:,i); 154 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 155 | B0t(:,i)=[xc;yc;zc]; 156 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 157 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 158 | g=O1P_bcs-O1G; 159 | GP=g/norm(g,2); 160 | GS=Rot'*OS_d(:,i); % Sun vector in bcs 161 | normal=(GS+GP)/norm((GS+GP),2); 162 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt global coordinate system 163 | n2=-yc/rp; 164 | o1=n2; 165 | o2=n1-(2*xc/rp); 166 | Trans_matr = [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 167 | n2 o2 a2 yc; 168 | n3 o3 a3 zc; 169 | 0 0 0 1] ; 170 | B0p1 = Trans_matr*[GS1;1]; % w.r.t base coordinate system 171 | l1_dir = B0p1-[O1R1;1]; 172 | l1(i,:)=norm(l1_dir,2); 173 | 174 | B0p2 = Trans_matr*[GS2;1]; 175 | l2_dir = B0p2-[O1R2;1]; 176 | l2(i,:)=norm(l2_dir,2); 177 | 178 | B0p3 = Trans_matr*[GS3;1]; 179 | l3_dir = B0p3-[O1R3;1]; 180 | l3(i,:)=norm(l3_dir,2); 181 | 182 | Act_req = cat(1, Act_req, [tme(i), 1000*(l1(i)-l10) 1000*(l2(i)-l20) 1000*(l3(i)-l30)]); 183 | end 184 | % in 1 second the actuator moves 2.4 mm when the speed is 200(bits). 0-0 velocity and 255-max velocity 185 | Act_tme = [Act_req(:,1) Act_req(:,2)/2.4 Act_req(:,3)/2.4 Act_req(:,4)/2.4 ]; 186 | L = [l1 l2 l3]; 187 | 188 | for i=1:length(Act_tme)-1 189 | act(i,:) = Act_tme(i+1,:) - Act_tme(i,:); 190 | end 191 | act(:,1) = []; 192 | act = cat(1, Act_tme(1,2:end), act); 193 | Act_time = [Act_tme(4,:); Act_tme(end,:); Act_tme(end,:)-Act_tme(4,:) ]; 194 | 195 | 196 | 197 | 198 | % Arduino 199 | global ard 200 | delete(instrfind({'PORT'},{'COM7'})) 201 | ard = arduino('COM7'); 202 | % Pins 4,5,6,7,8,9 are PWM pins. Motor 1 uses 4 and 5, Motor 2 uses 6 and 7 203 | % and Motor 3 uses 8 and 9. 204 | % Pins 31,32,33,34,35 are digital pins. Motor 1 uses 31 and NOT gate, Motor 205 | % 2 uses 32 and 33 and Motor 3 uses 34 and 35 206 | % Pins 2,3,18,19,20,21 are used for encoder input signals. Motor 1 uses 2 207 | % and 3, Motor 2 uses 18 and 19 and Motor 3 uses 20 and 21 208 | ard.pinMode(4,'OUTPUT'); 209 | ard.pinMode(5,'OUTPUT'); 210 | ard.pinMode(6, 'OUTPUT'); 211 | ard.pinMode(7,'OUTPUT'); 212 | ard.pinMode(8,'OUTPUT'); 213 | ard.pinMode(9, 'OUTPUT'); 214 | 215 | ard.pinMode(30,'OUTPUT'); 216 | ard.pinMode(31,'OUTPUT'); 217 | ard.pinMode(32,'OUTPUT'); 218 | ard.pinMode(33, 'OUTPUT'); 219 | ard.pinMode(34,'OUTPUT'); 220 | ard.pinMode(35,'OUTPUT'); 221 | 222 | % clear all 223 | % clc 224 | % global fileID 225 | % fileID = fopen('EXP.txt','w'); 226 | 227 | % act = [-3,2,1; 228 | % 3,1,-2.0]; 229 | [rw, cl] = size(act); 230 | [sort_act, Ind_act] = sort(abs(act),2); 231 | for i=1:rw 232 | tic 233 | while(toc <= sort_act(i,1)) 234 | RunMotor1(act(i,1)) 235 | RunMotor2(act(i,2)) 236 | RunMotor3(act(i,3)) 237 | end 238 | MOTORSTOP 239 | % fprintf(fileID, 'Elapsed Time is %8.3f \n',(toc)) 240 | % pause(1) 241 | % fprintf(fileID, 'AfterPause,Elapsed Time is %4.2f \n',toc) 242 | tic 243 | while (toc <= (sort_act(i,2)-sort_act(i,1))) 244 | if Ind_act(i,2) == 1 245 | RunMotor1(act(i,1)) 246 | elseif Ind_act(i,2) == 2 247 | RunMotor2(act(i,2)) 248 | elseif Ind_act(i,2) == 3 249 | RunMotor3(act(i,3)) 250 | end 251 | 252 | if Ind_act(i,3) == 1 253 | RunMotor1(act(i,1)) 254 | elseif Ind_act(i,3) == 2 255 | RunMotor2(act(i,2)) 256 | elseif Ind_act(i,3) == 3 257 | RunMotor3(act(i,3)) 258 | end 259 | end 260 | MOTORSTOP 261 | % fprintf(fileID, 'Elapsed Time is %4.2f \n',toc) 262 | % pause(1) 263 | % fprintf(fileID, 'After pause,Elapsed Time is %4.2f \n',toc) 264 | tic 265 | while (toc <= (sort_act(i,3)-sort_act(i,2))) 266 | if Ind_act(i,3) == 1 267 | RunMotor1(act(i,1)) 268 | elseif Ind_act(i,3) == 2 269 | RunMotor2(act(i,2)) 270 | elseif Ind_act(i,3) == 3 271 | RunMotor3(act(i,3)) 272 | end 273 | end 274 | MOTORSTOP 275 | % fprintf(fileID, 'Elapsed Time is %4.2f \n',toc) 276 | pause 277 | % fprintf(fileID, 'AfterPause,Elapsed Time is %4.2f \n',toc) 278 | end 279 | % fclose(fileID); 280 | % tt = timer('StartDelay', 2, 'Period', 5, ... 281 | % 'ExecutionMode', 'fixedRate'); 282 | % tic 283 | % tt.TimerFcn = @(x,y)RunMotor(act); 284 | % % 285 | % % tt.StartFcn = @(~,thisEvent)disp([thisEvent.Type ' executed '... 286 | % % datestr(thisEvent.Data.time,'dd-mmm-yyyy HH:MM:SS.FFF')]); 287 | % % tt.TimerFcn = @(~,thisEvent)disp([thisEvent.Type ' executed '... 288 | % % datestr(thisEvent.Data.time,'dd-mmm-yyyy HH:MM:SS.FFF')]); 289 | % % tt.StopFcn = @(~,thisEvent)disp([thisEvent.Type ' executed '... 290 | % % datestr(thisEvent.Data.time,'dd-mmm-yyyy HH:MM:SS.FFF')]); 291 | % % tt.Period = 3; 292 | % % tt.TasksToExecute = 3; 293 | % % tt.ExecutionMode = 'fixedRate'; 294 | % start(tt) 295 | % stop(tt) 296 | % delete(tt) -------------------------------------------------------------------------------- /src/expt/error/TEST_Error.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | home 5 | set(0,'defaulttextinterpreter','latex') 6 | % set(0,'DefaultAxesFontName', 'Times New Roman') 7 | set(0,'DefaultAxesFontSize', 25) 8 | set(0,'DefaultAxesFontWeight', 'Bold') 9 | 10 | % Change default text fonts. 11 | % set(0,'DefaultTextFontname', 'Times New Roman') 12 | set(0,'DefaultTextFontSize', 13) 13 | 14 | %% 15 | % % Bangalore Latitude 16 | lad=12; 17 | lam=58; 18 | las=13; 19 | 20 | % Bangalore Longitude 21 | lod=77; 22 | lom=33; 23 | los=37; 24 | 25 | latitude=lad+(lam+las/60)/60; % in degrees 26 | longitude=lod+(lom+los/60)/60; % in degrees 27 | %% 28 | % Rajasthan Coordinates 29 | % latitude = 27.0238; 30 | % longitude = 74.2179; 31 | %% 32 | 33 | year=2016; 34 | month=3; 35 | day=20; 36 | d=date(year,month,day); 37 | 38 | time_incre=360; 39 | LT=0:time_incre:24*3600; 40 | 41 | global OP rp zc Rot t m 42 | psi= 0; 43 | rad= 50; 44 | tx=rad*cosd(psi);ty=rad*sind(psi);tz=0; t=[tx;ty;tz]; 45 | a=0;b=0;c=25; 46 | % OP=[a;b;c]; 47 | %% 48 | zeta=psi; % the normal (lying in a plane parallel to XY plane) to the receiver is at an angle zeta to the X axis 49 | % Trans_Rec = Rz_90*Rx_90*Ry_zeta 50 | 51 | Rz_90= [ cosd(90) -sind(90) 0 ; 52 | sind(90) cosd(90) 0; 53 | 0 0 1]; 54 | Rx_90 = [1 0 0 ; 55 | 0 cosd(90) -sind(90); 56 | 0 sind(90) cosd(90)]; 57 | Ry_zeta = [cosd(zeta) 0 sind(zeta); 58 | 0 1 0 ; 59 | -sind(zeta) 0 cosd(zeta)]; 60 | 61 | Rot_Rec = Rz_90*Rx_90*Ry_zeta; 62 | Trans_Rec = [Rot_Rec,[a;b;c];[0 0 0 1]]; 63 | %% 64 | 65 | OP_rec_coord = [0.15;0.35;0.0]; % OP in receiver coordinates. 66 | OP = Trans_Rec * [OP_rec_coord;1]; % OP in gcs 67 | OP(4)=[]; 68 | zc=2; 69 | % zc = 0.2365; 70 | alpha=120; 71 | beta=120; 72 | mirror_size = 1.6; % 2m x 2m square mirror 73 | rp = mirror_size/4; 74 | rb = rp; 75 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 76 | %% Plot of azimuth and elevation angles 77 | v=8/time_incre*3600+1; % start time for the plots 78 | w=17/time_incre*3600+1; % end time for the plots 79 | 80 | figure(1) 81 | plot(LT(v:w)/3600,elevation((v: w)),'r','LineWidth',4) 82 | hold on 83 | plot(LT(v:w)/3600,azimuth((v: w)),'b','LineWidth',4) 84 | xlabel('Time \it{t / hr}') 85 | ylabel('$Sun vector angles ~ \it{\theta / (^0)}$') 86 | lgnd=legend('Elevation',' Azimuth'); 87 | grid on 88 | %% 89 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 90 | OS=[xs;ys;zs]; 91 | 92 | gamma=0; % Rotation of the base platform w.r.t the global coordinate system. 93 | Rot=[ cosd(gamma) -sind(gamma) 0; 94 | sind(gamma) cosd(gamma) 0; 95 | 0 0 1]; 96 | x0 = [.7;-.6;0.1;-0.7;-0.01]; % works for psi=0,45,90,135,180,225,270,315 degrees 97 | 98 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; % works only for psi=45 degree 99 | 100 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 101 | % options = optimoptions('fsolve','Jacobian','off'); 102 | 103 | for i=1:length(v:w) 104 | zz=v-1+i; 105 | fhandle=@(x)for_mainprog_3RPS(x,OS(:,zz)); 106 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 107 | x0=xval(:,i); 108 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 109 | O1G(:,i)=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 110 | O1P_bcs = Rot'*(OP-t); % O1P_bcs = O1P described in bcs 111 | g = O1P_bcs-O1G(:,i); % wrt bcs 112 | GP_bcs(:,i)=g/norm(g,2); % wrt bcs 113 | GS=Rot'*OS(:,zz); % Sun vector in bcs 114 | normal=(GS+GP_bcs(:,i))/norm((GS+GP_bcs(:,i)),2); % in bcs 115 | % GP_test(:,i) = (normal*norm((GS+GP_bcs(:,i)),2)-GS)/norm(((normal*norm((GS+GP_bcs(:,i)),2)-GS)),2); 116 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt base coordinate system 117 | n2=-yc/rp; 118 | o1=n2; 119 | o2=n1-(2*xc/rp); 120 | R(:,:,i) = [n1 o1 a1 ; % rotation which takes mirror to base coordinate system 121 | n2 o2 a2 ; 122 | n3 o3 a3 ] ; 123 | Trans_matr(:,:,i)= [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 124 | n2 o2 a2 yc; 125 | n3 o3 a3 zc; 126 | 0 0 0 1] ; 127 | end 128 | 129 | TransmatrN=[Rot,t;[0 0 0 1]]; % transformation which takes base to global coordinate system 130 | 131 | %% Just one mirror of dimension 2x2 132 | s = 1.25; % square receiver's half length 133 | m = mirror_size/2; % square mirror of half length 1. Actual mirror dimension is 2m x 2m. 134 | PB1 = [s;s;0]; PB2 = [-s;s;0]; PB3 = [-s;-s;0]; PB4 = [s;-s;0]; % P is the centre of the receiver having coordinates [a;b;c]. 135 | % B1, B2, B3, B4 are the corners of the square receiver 136 | OB1 = Trans_Rec * [PB1;1]; OB2=Trans_Rec * [PB2;1]; OB3=Trans_Rec * [PB3;1]; OB4=Trans_Rec * [PB4;1]; 137 | 138 | Gm1= [m;m;0]; Gm2= [-m;m;0]; Gm3= [-m;-m;0]; Gm4= [m;-m;0]; % G is the centre of the mirror. 139 | % m1, m2, m3 and m4 are the corners of the mirror 140 | 141 | %% 142 | figure() 143 | mm=1; zz=0; 144 | options1 = optimoptions(@fmincon,'Algorithm','sqp'); 145 | % kg0 = [0.90; -0.016; -0.11; 0.70; -0.10; -0.30; O1G(1,1); O1G(2,1)]; 146 | kg0 = [0.90; -0.016; 0.11; 0.014; -0.1180; 0.90; O1G(1,1); O1G(2,1)]; 147 | % r0 = [0.1;-0.5;0.7]; 148 | for i=1:length(v:w) 149 | zz=v-1+i; 150 | f1 = @(kg)error_min(kg,a,b,c,OS(:,zz),zeta); 151 | f2 = @(kg)confuneq_error_min(kg); 152 | % [KG, fval] = fmincon(f1,kg0, [], [], [], [], [], [], f2,options1); 153 | [KG, fval] = fmincon(f1,kg0, [], [], [], [], [], [], f2); 154 | kg0=KG; 155 | KKG(:,i)=KG; 156 | end 157 | 158 | zz=0; 159 | %% 160 | for i=1:length(Trans_matr) 161 | zz=v-1+i; 162 | OG = t + Rot*O1G(:,i); % in gcs 163 | Om1 = OG + R(:,:,i)*Gm1; Om2 = OG + R(:,:,i)*Gm2; %% mirror corners in gcs 164 | Om3 = OG + R(:,:,i)*Gm3; Om4 = OG + R(:,:,i)*Gm4; 165 | 166 | % plot of receiver aperture 167 | l_Rec = plot3([OB1(1) OB2(1)], [OB1(2) OB2(2)], [OB1(3) OB2(3)],'c','LineWidth',3); 168 | hold on 169 | plot3([OB2(1) OB3(1)], [OB2(2) OB3(2)], [OB2(3) OB3(3)],'c','LineWidth',3) 170 | plot3([OB3(1) OB4(1)], [OB3(2) OB4(2)], [OB3(3) OB4(3)],'c','LineWidth',3) 171 | plot3([OB4(1) OB1(1)], [OB4(2) OB1(2)], [OB4(3) OB1(3)],'c','LineWidth',3) 172 | 173 | plot3([0 a],[0 b],[0,c],'b') % Receiver tower 174 | plot3([0 a],[0 b],[0,c],'b*') % Receiver tower center 175 | 176 | % % plot mirror 177 | plot3([Om1(1) Om2(1)], [Om1(2) Om2(2)], [Om1(3) Om2(3)],'g') 178 | plot3([Om2(1) Om3(1)], [Om2(2) Om3(2)], [Om2(3) Om3(3)],'g') 179 | plot3([Om3(1) Om4(1)], [Om3(2) Om4(2)], [Om3(3) Om4(3)],'g') 180 | plot3([Om4(1) Om1(1)], [Om4(2) Om1(2)], [Om4(3) Om1(3)],'g') 181 | plot3(OG(1),OG(2),OG(3),'g*') % centroid of mirror 182 | 183 | % OHp = (Om1 + constant * unit reflected ray) should hit the receiver 184 | % aperture. OP is a point on the receiver surface where the sun ray 185 | % hitting the centre of the mirror supposed to fall. OHp - OP is a 186 | % vector in the plane of the receiver. The dot product of OHp and 187 | % normal to the receiver plane is zero. This is used to find the 188 | % constant 189 | 190 | GP = Rot*GP_bcs(:,i); % reflected ray in gcs 191 | k1=(cosd(zeta)*(a-Om1(1)) + sind(zeta)* (b-Om1(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 192 | k2=(cosd(zeta)*(a-Om2(1)) + sind(zeta)* (b-Om2(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 193 | k3=(cosd(zeta)*(a-Om3(1)) + sind(zeta)* (b-Om3(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 194 | k4=(cosd(zeta)*(a-Om4(1)) + sind(zeta)* (b-Om4(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 195 | 196 | % hitting points on the receiver 197 | hp1=Om1+k1*GP; % in gcs 198 | hp2=Om2+k2*GP; % in gcs 199 | hp3=Om3+k3*GP; % in gcs 200 | hp4=Om4+k4*GP; % in gcs 201 | centroid = (hp1+hp2+hp3+hp4)/4; 202 | plot3([0 centroid(1)], [0 centroid(2)], [0 centroid(3)], 'g*') 203 | 204 | % Plot of image on the receiver 205 | l_3RPS = plot3([hp1(1) hp2(1)] ,[hp1(2) hp2(2)], [hp1(3) hp2(3)], 'g','LineWidth',1); 206 | plot3([hp2(1) hp3(1)] ,[hp2(2) hp3(2)], [hp2(3) hp3(3)], 'g','LineWidth',1) 207 | plot3([hp3(1) hp4(1)] ,[hp3(2) hp4(2)], [hp3(3) hp4(3)], 'g','LineWidth',1) 208 | plot3([hp4(1) hp1(1)] ,[hp4(2) hp1(2)], [hp4(3) hp1(3)], 'g','LineWidth',1) 209 | 210 | % 211 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 212 | % %%%%%%%%%%%%%% ERROR CORRECTION MODEL %%%%%%%%%%%%%%%%%%%% 213 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 214 | n1N = KKG(1,i); n3N = KKG(2,i); o3N = KKG(3,i); 215 | a1N = KKG(4,i); a2N = KKG(5,i); a3N = KKG(6,i); % wrt base coordinate system 216 | xcN = KKG(7,i); ycN = KKG(8,i); 217 | n2N = -ycN/rp; 218 | o1N = n2N; 219 | o2N = n1N-(2*xcN/rp); 220 | O1GN = O1G(:,i); % new G in bcs 221 | OGN = t+Rot*O1GN; % This is the new point G in gcs 222 | RN = [n1N o1N a1N ; % Error corrected rotation matrix which 223 | n2N o2N a2N ; % takes mirror to base coordinate system 224 | n3N o3N a3N ]; 225 | normalN = [a1N;a2N;a3N]; % in bcs 226 | 227 | Om1N = OGN + RN*Gm1; Om2N = OGN + RN*Gm2; %% mirror corners in gcs 228 | Om3N = OGN + RN*Gm3; Om4N = OGN + RN*Gm4; 229 | % plot mirror 230 | plot3([Om1N(1) Om2N(1)], [Om1N(2) Om2N(2)], [Om1N(3) Om2N(3)],'r') 231 | plot3([Om2N(1) Om3N(1)], [Om2N(2) Om3N(2)], [Om2N(3) Om3N(3)],'r') 232 | plot3([Om3N(1) Om4N(1)], [Om3N(2) Om4N(2)], [Om3N(3) Om4N(3)],'r') 233 | plot3([Om4N(1) Om1N(1)], [Om4N(2) Om1N(2)], [Om4N(3) Om1N(3)],'r') 234 | plot3(OGN(1),OGN(2),OGN(3),'r*') % centroid of mirror 235 | % 236 | % 237 | %% New reflected ray 238 | % fhandle=@(r)Refl_ray(r,Rot'*OS(:,zz),normalN); 239 | % [GPN,fvald] = fsolve(fhandle,r0,options); 240 | % r0=GPN; 241 | % %%%% 242 | axy = cross(Rot'*OS(:,zz),[a1N;a2N;a3N]); % IN bcs 243 | axs = axy/norm(axy,2); % IN bcs 244 | angle1 = acos(dot(Rot'*OS(:,zz),[a1N;a2N;a3N])); % IN bcs); %%% RADIANS 245 | k1=axs(1); k2 = axs(2); k3 = axs(3); 246 | 247 | r11 = k1^2*(1-cos(angle1)) + cos(angle1); 248 | r12 = k1*k2*(1-cos(angle1))- k3*sin(angle1); 249 | r13 = k3*k1*(1-cos(angle1))+ k2*sin(angle1); 250 | r21 = k1*k2*(1-cos(angle1))+ k3*sin(angle1); 251 | r22 = k2^2*(1-cos(angle1)) + cos(angle1); 252 | r23 = k3*k2*(1-cos(angle1))- k1*sin(angle1); 253 | r31 = k3*k1*(1-cos(angle1))- k2*sin(angle1); 254 | r32 = k3*k2*(1-cos(angle1))+ k1*sin(angle1); 255 | r33 = k3^2*(1-cos(angle1)) + cos(angle1); 256 | Rot_ref = [r11 r12 r13; 257 | r21 r22 r23; 258 | r31 r32 r33]; 259 | GPN = Rot_ref*[a1N;a2N;a3N] ; 260 | %%%%%%%%%%%%%%%%%%%% 261 | %% 262 | kon=5; kons=0.8; 263 | ref_ray = plot3([OGN(1) OGN(1)+kon*GPN(1)],[OGN(2) OGN(2)+kon*GPN(2)],[OGN(3) OGN(3)+kon*GPN(3)],'b-','LineWidth',2); 264 | % % ref_rayd = plot3([OGN(1) OGN(1)+kon*GPNd(1)],[OGN(2) OGN(2)+kon*GPNd(2)],[OGN(3) OGN(3)+kon*GPNd(3)],'g-','LineWidth',2); 265 | % 266 | sun = plot3([OGN(1) OGN(1)+kon*OS(1,zz)],[OGN(2) OGN(2)+kon*OS(2,zz)],[OGN(3) OGN(3)+kon*OS(3,zz)],'k-','LineWidth',2); 267 | nrml = plot3([OGN(1) OGN(1)+kon*normalN(1)],[OGN(2) OGN(2)+kon*normalN(2)],[OGN(3) OGN(3)+kon*normalN(3)],'r-','LineWidth',2); 268 | xaxis = plot3([OGN(1) OGN(1)+kons*RN(1,1)],[OGN(2) OGN(2)+kons*RN(2,1)],[OGN(3) OGN(3)+kons*RN(3,1)],'c-','LineWidth',2); 269 | yaxis = plot3([OGN(1) OGN(1)+kons*RN(1,2)],[OGN(2) OGN(2)+kons*RN(2,2)],[OGN(3) OGN(3)+kons*RN(3,2)],'m-','LineWidth',2); 270 | 271 | k1N=(cosd(zeta)*(a-Om1N(1)) + sind(zeta)* (b-Om1N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 272 | k2N=(cosd(zeta)*(a-Om2N(1)) + sind(zeta)* (b-Om2N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 273 | k3N=(cosd(zeta)*(a-Om3N(1)) + sind(zeta)* (b-Om3N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 274 | k4N=(cosd(zeta)*(a-Om4N(1)) + sind(zeta)* (b-Om4N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 275 | 276 | % hitting points on the receiver 277 | hp1N=Om1N+k1N*GPN; % in gcs 278 | hp2N=Om2N+k2N*GPN; % in gcs 279 | hp3N=Om3N+k3N*GPN; % in gcs 280 | hp4N=Om4N+k4N*GPN; % in gcs 281 | centroidN = (hp1N+hp2N+hp3N+hp4N)/4; 282 | distN(i) = norm((centroidN-[a;b;c]),2); 283 | 284 | plot3([0 centroidN(1)], [0 centroidN(2)], [0 centroidN(3)], 'r*') 285 | 286 | % Plot of image on the receiver 287 | l_3RPSN = plot3([hp1N(1) hp2N(1)] ,[hp1N(2) hp2N(2)], [hp1N(3) hp2N(3)], 'r','LineWidth',1); 288 | plot3([hp2N(1) hp3N(1)] ,[hp2N(2) hp3N(2)], [hp2N(3) hp3N(3)], 'r','LineWidth',1) 289 | plot3([hp3N(1) hp4N(1)] ,[hp3N(2) hp4N(2)], [hp3N(3) hp4N(3)], 'r','LineWidth',1) 290 | plot3([hp4N(1) hp1N(1)] ,[hp4N(2) hp1N(2)], [hp4N(3) hp1N(3)], 'r','LineWidth',1) 291 | 292 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 293 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 294 | 295 | axis([tx-3 tx+3 ty-3 ty+3 0 tz+3]); % to see the mirror 296 | % axis([-1.5 1.5 -1.5 1.5 c-3 c+3]); %view(90,0) % to see the Receiver 297 | view(90,0) 298 | axis square 299 | % axis equal 300 | xlabel('{\boldmath $ X, East$}') 301 | ylabel('{\boldmath $ Y, North$}') 302 | zlabel('{\boldmath $ Z, Zenith$}') 303 | grid on 304 | hold off 305 | M(mm)=getframe(); 306 | % h1=legend([l_Rec l_AzEl l_TA],'Receiver','AzEl', 'TA',[300,67,3,4]); 307 | % set(h1,'Interpreter','latex'); 308 | % M = getframe; 309 | % writeVideo(writerObj,M); 310 | mm=mm+1; 311 | end 312 | %% 313 | dist = norm((OP-[a;b;c])); 314 | -------------------------------------------------------------------------------- /src/expt/error/ERROR.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | home 5 | set(0,'defaulttextinterpreter','latex') 6 | % set(0,'DefaultAxesFontName', 'Times New Roman') 7 | set(0,'DefaultAxesFontSize', 25) 8 | set(0,'DefaultAxesFontWeight', 'Bold') 9 | 10 | % Change default text fonts. 11 | % set(0,'DefaultTextFontname', 'Times New Roman') 12 | set(0,'DefaultTextFontSize', 13) 13 | 14 | %% 15 | % % Bangalore Latitude 16 | lad=12; 17 | lam=58; 18 | las=13; 19 | 20 | % Bangalore Longitude 21 | lod=77; 22 | lom=33; 23 | los=37; 24 | 25 | latitude=lad+(lam+las/60)/60; % in degrees 26 | longitude=lod+(lom+los/60)/60; % in degrees 27 | %% 28 | % Rajasthan Coordinates 29 | % latitude = 27.0238; 30 | % longitude = 74.2179; 31 | %% 32 | 33 | year=2016; 34 | month=3; 35 | day=20; 36 | d=date(year,month,day); 37 | 38 | time_incre=360; 39 | LT=0:time_incre:24*3600; 40 | 41 | global OP rp zc Rot t m 42 | psi= 0; 43 | rad= 50; 44 | tx=rad*cosd(psi);ty=rad*sind(psi);tz=0; t=[tx;ty;tz]; 45 | a=0;b=0;c=25; 46 | % OP=[a;b;c]; 47 | %% 48 | zeta=psi; % the normal (lying in a plane parallel to XY plane) to the receiver is at an angle zeta to the X axis 49 | % Trans_Rec = Rz_90*Rx_90*Ry_zeta 50 | 51 | Rz_90= [ cosd(90) -sind(90) 0 ; 52 | sind(90) cosd(90) 0; 53 | 0 0 1]; 54 | Rx_90 = [1 0 0 ; 55 | 0 cosd(90) -sind(90); 56 | 0 sind(90) cosd(90)]; 57 | Ry_zeta = [cosd(zeta) 0 sind(zeta); 58 | 0 1 0 ; 59 | -sind(zeta) 0 cosd(zeta)]; 60 | 61 | Rot_Rec = Rz_90*Rx_90*Ry_zeta; 62 | Trans_Rec = [Rot_Rec,[a;b;c];[0 0 0 1]]; 63 | %% 64 | 65 | OP_rec_coord = [0.15;0.35;0.0]; % OP in receiver coordinates. 66 | OP = Trans_Rec * [OP_rec_coord;1]; % OP in gcs 67 | OP(4)=[]; 68 | zc=2; 69 | % zc = 0.2365; 70 | alpha=120; 71 | beta=120; 72 | mirror_size = 1.6; % 2m x 2m square mirror 73 | rp = mirror_size/4; 74 | rb = rp; 75 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 76 | %% Plot of azimuth and elevation angles 77 | v=8/time_incre*3600+1; % start time for the plots 78 | w=17/time_incre*3600+1; % end time for the plots 79 | 80 | figure(1) 81 | plot(LT(v:w)/3600,elevation((v: w)),'r','LineWidth',4) 82 | hold on 83 | plot(LT(v:w)/3600,azimuth((v: w)),'b','LineWidth',4) 84 | xlabel('Time \it{t / hr}') 85 | ylabel('$Sun vector angles ~ \it{\theta / (^0)}$') 86 | lgnd=legend('Elevation',' Azimuth'); 87 | grid on 88 | %% 89 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 90 | OS=[xs;ys;zs]; 91 | 92 | gamma=0; % Rotation of the base platform w.r.t the global coordinate system. 93 | Rot=[ cosd(gamma) -sind(gamma) 0; 94 | sind(gamma) cosd(gamma) 0; 95 | 0 0 1]; 96 | x0 = [.7;-.6;0.1;-0.7;-0.01]; % works for psi=0,45,90,135,180,225,270,315 degrees 97 | 98 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; % works only for psi=45 degree 99 | 100 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 101 | % options = optimoptions('fsolve','Jacobian','off'); 102 | 103 | for i=1:length(v:w) 104 | zz=v-1+i; 105 | fhandle=@(x)for_mainprog_3RPS(x,OS(:,zz)); 106 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 107 | x0=xval(:,i); 108 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 109 | O1G(:,i)=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 110 | O1P_bcs = Rot'*(OP-t); % O1P_bcs = O1P described in bcs 111 | g = O1P_bcs-O1G(:,i); % wrt bcs 112 | GP_bcs(:,i)=g/norm(g,2); % wrt bcs 113 | GS=Rot'*OS(:,zz); % Sun vector in bcs 114 | normal=(GS+GP_bcs(:,i))/norm((GS+GP_bcs(:,i)),2); % in bcs 115 | % GP_test(:,i) = (normal*norm((GS+GP_bcs(:,i)),2)-GS)/norm(((normal*norm((GS+GP_bcs(:,i)),2)-GS)),2); 116 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt base coordinate system 117 | n2=-yc/rp; 118 | o1=n2; 119 | o2=n1-(2*xc/rp); 120 | R(:,:,i) = [n1 o1 a1 ; % rotation which takes mirror to base coordinate system 121 | n2 o2 a2 ; 122 | n3 o3 a3 ] ; 123 | Trans_matr(:,:,i)= [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 124 | n2 o2 a2 yc; 125 | n3 o3 a3 zc; 126 | 0 0 0 1] ; 127 | end 128 | 129 | TransmatrN=[Rot,t;[0 0 0 1]]; % transformation which takes base to global coordinate system 130 | 131 | %% Just one mirror of dimension 2x2 132 | s = 1.25; % square receiver's half length 133 | m = mirror_size/2; % square mirror of half length 1. Actual mirror dimension is 2m x 2m. 134 | PB1 = [s;s;0]; PB2 = [-s;s;0]; PB3 = [-s;-s;0]; PB4 = [s;-s;0]; % P is the centre of the receiver having coordinates [a;b;c]. 135 | % B1, B2, B3, B4 are the corners of the square receiver 136 | OB1 = Trans_Rec * [PB1;1]; OB2=Trans_Rec * [PB2;1]; OB3=Trans_Rec * [PB3;1]; OB4=Trans_Rec * [PB4;1]; 137 | 138 | Gm1= [m;m;0]; Gm2= [-m;m;0]; Gm3= [-m;-m;0]; Gm4= [m;-m;0]; % G is the centre of the mirror. 139 | % m1, m2, m3 and m4 are the corners of the mirror 140 | figure() 141 | % set(gca,'nextplot','replacechildren'); 142 | mm=0; zz=0; 143 | r0 = [0.1;-0.5;0.7]; 144 | x0 = [.9;-.016;0.11;0.007;-0.01]; % works for psi=0,45,90,135,180,225,270,315 degrees 145 | for i=1:length(Trans_matr) 146 | zz=v-1+i; 147 | OG = t + Rot*O1G(:,i); % in gcs 148 | Om1 = OG + R(:,:,i)*Gm1; Om2 = OG + R(:,:,i)*Gm2; %% mirror corners in gcs 149 | Om3 = OG + R(:,:,i)*Gm3; Om4 = OG + R(:,:,i)*Gm4; 150 | 151 | % plot of receiver aperture 152 | l_Rec = plot3([OB1(1) OB2(1)], [OB1(2) OB2(2)], [OB1(3) OB2(3)],'c','LineWidth',3); 153 | hold on 154 | plot3([OB2(1) OB3(1)], [OB2(2) OB3(2)], [OB2(3) OB3(3)],'c','LineWidth',3) 155 | plot3([OB3(1) OB4(1)], [OB3(2) OB4(2)], [OB3(3) OB4(3)],'c','LineWidth',3) 156 | plot3([OB4(1) OB1(1)], [OB4(2) OB1(2)], [OB4(3) OB1(3)],'c','LineWidth',3) 157 | 158 | plot3([0 a],[0 b],[0,c],'b') % Receiver tower 159 | plot3([0 a],[0 b],[0,c],'b*') % Receiver tower center 160 | 161 | % % plot mirror 162 | plot3([Om1(1) Om2(1)], [Om1(2) Om2(2)], [Om1(3) Om2(3)],'g') 163 | plot3([Om2(1) Om3(1)], [Om2(2) Om3(2)], [Om2(3) Om3(3)],'g') 164 | plot3([Om3(1) Om4(1)], [Om3(2) Om4(2)], [Om3(3) Om4(3)],'g') 165 | plot3([Om4(1) Om1(1)], [Om4(2) Om1(2)], [Om4(3) Om1(3)],'g') 166 | plot3(OG(1),OG(2),OG(3),'g*') % centroid of mirror 167 | 168 | % OHp = (Om1 + constant * unit reflected ray) should hit the receiver 169 | % aperture. OP is a point on the receiver surface where the sun ray 170 | % hitting the centre of the mirror supposed to fall. OHp - OP is a 171 | % vector in the plane of the receiver. The dot product of OHp and 172 | % normal to the receiver plane is zero. This is used to find the 173 | % constant 174 | 175 | GP = Rot*GP_bcs(:,i); % reflected ray in gcs 176 | k1=(cosd(zeta)*(a-Om1(1)) + sind(zeta)* (b-Om1(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 177 | k2=(cosd(zeta)*(a-Om2(1)) + sind(zeta)* (b-Om2(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 178 | k3=(cosd(zeta)*(a-Om3(1)) + sind(zeta)* (b-Om3(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 179 | k4=(cosd(zeta)*(a-Om4(1)) + sind(zeta)* (b-Om4(2)))/( GP(1)*cosd(zeta) + GP(2)*sind(zeta) ); 180 | 181 | % hitting points on the receiver 182 | hp1=Om1+k1*GP; % in gcs 183 | hp2=Om2+k2*GP; % in gcs 184 | hp3=Om3+k3*GP; % in gcs 185 | hp4=Om4+k4*GP; % in gcs 186 | centroid = (hp1+hp2+hp3+hp4)/4; 187 | plot3([0 centroid(1)], [0 centroid(2)], [0 centroid(3)], 'g*') 188 | 189 | % Plot of image on the receiver 190 | l_3RPS = plot3([hp1(1) hp2(1)] ,[hp1(2) hp2(2)], [hp1(3) hp2(3)], 'g','LineWidth',1); 191 | plot3([hp2(1) hp3(1)] ,[hp2(2) hp3(2)], [hp2(3) hp3(3)], 'g','LineWidth',1) 192 | plot3([hp3(1) hp4(1)] ,[hp3(2) hp4(2)], [hp3(3) hp4(3)], 'g','LineWidth',1) 193 | plot3([hp4(1) hp1(1)] ,[hp4(2) hp1(2)], [hp4(3) hp1(3)], 'g','LineWidth',1) 194 | 195 | 196 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 197 | %%%%%%%%%%%%%% ERROR CORRECTION MODEL %%%%%%%%%%%%%%%%%%%% 198 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 199 | 200 | % To find actual normal 201 | Actual_refl_ray = (OP - OG)/norm((OP - OG),2); % actual reflected ray in gcs 202 | Ideal_refl_ray = ([a;b;c] - OG)/norm(([a;b;c] - OG),2); % ideal reflected ray in gcs 203 | Actual_normal = (Actual_refl_ray + OS(:,zz))/norm(((Actual_refl_ray + OS(:,zz))),2); % in gcs 204 | Ideal_normal = (Ideal_refl_ray + OS(:,zz))/norm(((Ideal_refl_ray + OS(:,zz))),2); % in gcs 205 | ax = cross(Actual_normal,Ideal_normal); % IN gcs 206 | axys = ax/norm(ax,2); % IN gcs 207 | angle = acos(dot(Actual_normal,Ideal_normal)); % IN gcs); %%% RADIANS 208 | k1=axys(1); k2 = axys(2); k3 = axys(3); 209 | 210 | r11 = k1^2*(1-cos(angle))+cos(angle); 211 | r12 = k1*k2*(1-cos(angle))-k3*sin(angle); 212 | r13 = k3*k1*(1-cos(angle))+k2*sin(angle); 213 | r21 = k1*k2*(1-cos(angle))+k3*sin(angle); 214 | r22 = k2^2*(1-cos(angle))+cos(angle); 215 | r23 = k3*k2*(1-cos(angle))-k1*sin(angle); 216 | r31 = k3*k1*(1-cos(angle))-k2*sin(angle); 217 | r32 = k3*k2*(1-cos(angle))+k1*sin(angle); 218 | r33 = k3^2*(1-cos(angle))+cos(angle); 219 | Rot_err = [r11 r12 r13; 220 | r21 r22 r23; 221 | r31 r32 r33]; 222 | Rot_err11(:,:,i) = Rot_err ; 223 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 224 | fhandle=@(x)for_mainprog_3RPS_test(x,Rot_err*R(:,3,i),O1G(:,i)); 225 | [xval,fval] = fsolve(fhandle,x0,options); 226 | x0=xval; 227 | n1N=xval(1);n3N=xval(2);o3N=xval(3); xcN=O1G(1,i); ycN=O1G(2,i); 228 | normalN = Rot_err*R(:,3,i); % in bcs 229 | a1N=normalN(1); a2N=normalN(2); a3N=normalN(3); % wrt base coordinate system 230 | n2N=-ycN/rp; 231 | o1N=n2N; 232 | o2N=n1N-(2*xcN/rp); 233 | O1GN(:,i)=O1G(:,i); % new G in bcs 234 | OGN = t+Rot*O1GN(:,i); % This is the new point G in gcs 235 | RN = [n1N o1N a1N ; % rotation which takes mirror to base coordinate system 236 | n2N o2N a2N ; 237 | n3N o3N a3N ]; 238 | RN11(:,:,i) = RN; 239 | Om1N = OGN + RN*Gm1; Om2N = OGN + RN*Gm2; %% mirror corners in gcs 240 | Om3N = OGN + RN*Gm3; Om4N = OGN + RN*Gm4; 241 | % plot mirror 242 | plot3([Om1N(1) Om2N(1)], [Om1N(2) Om2N(2)], [Om1N(3) Om2N(3)],'r') 243 | plot3([Om2N(1) Om3N(1)], [Om2N(2) Om3N(2)], [Om2N(3) Om3N(3)],'r') 244 | plot3([Om3N(1) Om4N(1)], [Om3N(2) Om4N(2)], [Om3N(3) Om4N(3)],'r') 245 | plot3([Om4N(1) Om1N(1)], [Om4N(2) Om1N(2)], [Om4N(3) Om1N(3)],'r') 246 | plot3(OGN(1),OGN(2),OGN(3),'r*') % centroid of mirror 247 | 248 | % New reflected ray 249 | fhandle=@(r)Refl_ray(r,Rot'*OS(:,zz),normalN); 250 | [GPN,fvald] = fsolve(fhandle,r0,options); 251 | r0=GPN; 252 | GPd(:,i) = GPN; 253 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 254 | % Alternate way to find reflected ray 255 | axy = cross(Rot'*OS(:,zz),normalN); % IN gcs 256 | axs = axy/norm(axy,2); % IN gcs 257 | angle1 = acos(dot(Rot'*OS(:,zz),normalN)); % IN gcs); %%% RADIANS 258 | k1=axs(1); k2 = axs(2); k3 = axs(3); 259 | 260 | r11 = k1^2*(1-cos(angle1))+cos(angle1); 261 | r12 = k1*k2*(1-cos(angle1))-k3*sin(angle1); 262 | r13 = k3*k1*(1-cos(angle1))+k2*sin(angle1); 263 | r21 = k1*k2*(1-cos(angle1))+k3*sin(angle1); 264 | r22 = k2^2*(1-cos(angle1))+cos(angle1); 265 | r23 = k3*k2*(1-cos(angle1))-k1*sin(angle1); 266 | r31 = k3*k1*(1-cos(angle1))-k2*sin(angle1); 267 | r32 = k3*k2*(1-cos(angle1))+k1*sin(angle1); 268 | r33 = k3^2*(1-cos(angle1))+cos(angle1); 269 | Rot_ref = [r11 r12 r13; 270 | r21 r22 r23; 271 | r31 r32 r33]; 272 | GPNd(:,i) = Rot_ref*normalN ; 273 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 274 | 275 | kon=5; kons=0.8; 276 | ref_ray = plot3([OGN(1) OGN(1)+kon*GPN(1)],[OGN(2) OGN(2)+kon*GPN(2)],[OGN(3) OGN(3)+kon*GPN(3)],'b-','LineWidth',2); 277 | % ref_rayd = plot3([OGN(1) OGN(1)+kon*GPNd(1)],[OGN(2) OGN(2)+kon*GPNd(2)],[OGN(3) OGN(3)+kon*GPNd(3)],'g-','LineWidth',2); 278 | 279 | sun = plot3([OGN(1) OGN(1)+kon*OS(1,zz)],[OGN(2) OGN(2)+kon*OS(2,zz)],[OGN(3) OGN(3)+kon*OS(3,zz)],'k-','LineWidth',2); 280 | nrml = plot3([OGN(1) OGN(1)+kon*normalN(1)],[OGN(2) OGN(2)+kon*normalN(2)],[OGN(3) OGN(3)+kon*normalN(3)],'r-','LineWidth',2); 281 | xaxis = plot3([OGN(1) OGN(1)+kons*RN(1,1)],[OGN(2) OGN(2)+kons*RN(2,1)],[OGN(3) OGN(3)+kons*RN(3,1)],'c-','LineWidth',2); 282 | yaxis = plot3([OGN(1) OGN(1)+kons*RN(1,2)],[OGN(2) OGN(2)+kons*RN(2,2)],[OGN(3) OGN(3)+kons*RN(3,2)],'m-','LineWidth',2); 283 | 284 | k1N=(cosd(zeta)*(a-Om1N(1)) + sind(zeta)* (b-Om1N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 285 | k2N=(cosd(zeta)*(a-Om2N(1)) + sind(zeta)* (b-Om2N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 286 | k3N=(cosd(zeta)*(a-Om3N(1)) + sind(zeta)* (b-Om3N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 287 | k4N=(cosd(zeta)*(a-Om4N(1)) + sind(zeta)* (b-Om4N(2)))/( GPN(1)*cosd(zeta) + GPN(2)*sind(zeta) ); 288 | 289 | % hitting points on the receiver 290 | hp1N=Om1N+k1N*GPN; % in gcs 291 | hp2N=Om2N+k2N*GPN; % in gcs 292 | hp3N=Om3N+k3N*GPN; % in gcs 293 | hp4N=Om4N+k4N*GPN; % in gcs 294 | centroidN = (hp1N+hp2N+hp3N+hp4N)/4; 295 | distN(i) = norm((centroidN-[a;b;c]),2); 296 | 297 | plot3([0 centroidN(1)], [0 centroidN(2)], [0 centroidN(3)], 'r*') 298 | 299 | % Plot of image on the receiver 300 | l_3RPSN = plot3([hp1N(1) hp2N(1)] ,[hp1N(2) hp2N(2)], [hp1N(3) hp2N(3)], 'r','LineWidth',1); 301 | plot3([hp2N(1) hp3N(1)] ,[hp2N(2) hp3N(2)], [hp2N(3) hp3N(3)], 'r','LineWidth',1) 302 | plot3([hp3N(1) hp4N(1)] ,[hp3N(2) hp4N(2)], [hp3N(3) hp4N(3)], 'r','LineWidth',1) 303 | plot3([hp4N(1) hp1N(1)] ,[hp4N(2) hp1N(2)], [hp4N(3) hp1N(3)], 'r','LineWidth',1) 304 | 305 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 306 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 307 | 308 | % axis([tx-3 tx+3 ty-3 ty+3 0 tz+3]); % to see the mirror 309 | axis([-1.5 1.5 -1.5 1.5 c-3 c+3]); %view(90,0) % to see the Receiver 310 | view(90,0) 311 | axis square 312 | % axis equal 313 | xlabel('{\boldmath $ X, East$}') 314 | ylabel('{\boldmath $ Y, North$}') 315 | zlabel('{\boldmath $ Z, Zenith$}') 316 | grid on 317 | hold off 318 | % M(mm)=getframe(gcf); 319 | % h1=legend([l_Rec l_AzEl l_TA],'Receiver','AzEl', 'TA',[300,67,3,4]); 320 | % set(h1,'Interpreter','latex'); 321 | M = getframe; 322 | % writeVideo(writerObj,M); 323 | % mm=mm+1; 324 | end 325 | dist = norm((OP-[a;b;c])); 326 | 327 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 328 | O1Rs=[O1R1 O1R2 O1R3]; 329 | 330 | GS1=[rp;0;0];GS2=[-.5*rp;sqrt(3)*rp/2;0];GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 331 | GSs=[GS1 GS2 GS3]; 332 | 333 | for i=1:length(R) 334 | B0p1=O1G(:,i)+R(:,:,i)*GS1; % w.r.t base coordinate system 335 | l1_dir=B0p1-O1R1; 336 | l1(i,:)=norm(l1_dir,2); 337 | 338 | B0p2=O1G(:,i)+R(:,:,i)*GS2; % w.r.t base coordinate system 339 | l2_dir=B0p2-O1R2; 340 | l2(i,:)=norm(l2_dir,2); 341 | 342 | B0p3 = O1G(:,i)+R(:,:,i)*GS3; % w.r.t base coordinate system 343 | l3_dir=B0p3-O1R3; 344 | l3(i,:)=norm(l3_dir,2); 345 | 346 | B0p1N=O1G(:,i)+RN11(:,:,i)*GS1; % w.r.t base coordinate system 347 | l1N_dir=B0p1N-O1R1; 348 | l1N(i,:)=norm(l1N_dir,2); 349 | 350 | B0p2N = O1G(:,i)+RN11(:,:,i)*GS2; % w.r.t base coordinate system 351 | l2N_dir=B0p2N-O1R2; 352 | l2N(i,:)=norm(l2N_dir,2); 353 | 354 | B0p3N = O1G(:,i)+RN11(:,:,i)*GS3; % w.r.t base coordinate system 355 | l3N_dir = B0p3N-O1R3; 356 | l3N(i,:) = norm(l3N_dir,2); 357 | 358 | end 359 | error_l1 = l1N - l1; error_l2 = l2N - l2; error_l3 = l3N - l3; 360 | %% plot of leg length 361 | figure () 362 | plot(LT(v:w)/3600,1000*error_l1,'r*','LineWidth',2) 363 | hold on 364 | plot(LT(v:w)/3600,error_l2,'g*','LineWidth',2) 365 | plot(LT(v:w)/3600,error_l3,'b*','LineWidth',2) 366 | 367 | xlabel('{\boldmath $Time, hrs$}') 368 | ylabel('{\boldmath $ leg length, mm$}') 369 | % title('leg length variation wrt time') 370 | legend('leg1','leg2', 'leg3') 371 | grid on 372 | 373 | -------------------------------------------------------------------------------- /src/3-rps/final_program1.m: -------------------------------------------------------------------------------- 1 | % this solves 5 equations in 5 unknowns 2 | %% VERY IMP: for some months like june, and some other months like december azimuth = 360-a(1). otherwise time vs az-El graph will not be continuous 3 | % As per the data sheet of chinese linear actuator( HF-TGE), the retracted length (when stroke is 4 | % 0) is 1250mm. So the leg length should not be lower than this value. 5 | % From 1250mm, it can go to a max distance of 2250mm (1000 mm stroke). So 6 | % the maximum of each of the three legs should not exceed 2250 mm. To 7 | % obtain stow position, all the three legs to have equal length. 8 | 9 | close all 10 | clear all 11 | clc 12 | home 13 | set(0,'defaulttextinterpreter','latex') 14 | % set(0,'DefaultAxesFontName', 'Times New Roman') 15 | set(0,'DefaultAxesFontSize', 20) 16 | % set(0,'DefaultAxesFontWeight', 'Bold') 17 | 18 | % Change default text fonts. 19 | % set(0,'DefaultTextFontname', 'Times New Roman') 20 | set(0,'DefaultTextFontSize', 20) 21 | 22 | % Bangalore Latitude 23 | lad=12; 24 | lam=58; 25 | las=13; 26 | 27 | % Bangalore Longitude 28 | lod=77; 29 | lom=33; 30 | los=37; 31 | % 32 | % latitude=lad+(lam+las/60)/60; % in degrees 33 | % longitude=lod+(lom+los/60)/60; % in degrees 34 | 35 | % 36 | % Rajasthan Coordinates 37 | latitude = 27.0238; 38 | longitude = 74.2179; 39 | 40 | year=2016; 41 | month=9; 42 | day=22; 43 | d=date(year,month,day); 44 | % d=[79 141 266 356]; 45 | % d=79; 46 | time_incre=60; 47 | LT=0:time_incre:24*3600; 48 | 49 | global OP rp zc t Rot 50 | psi=30; 51 | rad=100; 52 | tx=rad*cosd(psi);ty=rad*sind(psi);tz=0; t=[tx;ty;tz]; 53 | a=0;b=0;c=65; 54 | % rb=0.36; 55 | rb = 0.50; 56 | rp=0.50; 57 | [elevation, azimuth]=alpha_azimuthcalc(LT,d,longitude,latitude); 58 | OP=[a;b;c]; 59 | [xs ,ys, zs]=sunposition(elevation, azimuth); % xs,ys,zs are coordinates of the sun 60 | OS=[xs;ys;zs]; 61 | zc=2; 62 | O1R1=[rb;0;0];O1R2=[-.5*rb;sqrt(3)*rb/2;0];O1R3=[-.5*rb;-sqrt(3)*rb/2;0]; 63 | O1Rs=[O1R1 O1R2 O1R3]; 64 | 65 | GS1=[rp;0;0];GS2=[-.5*rp;sqrt(3)*rp/2;0];GS3=[-.5*rp;-sqrt(3)*rp/2;0]; 66 | GSs=[GS1 GS2 GS3]; 67 | %% 68 | gamma=0; 69 | Rot=[ cosd(gamma) -sind(gamma) 0; 70 | sind(gamma) cosd(gamma) 0; 71 | 0 0 1]; 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 73 | % x0 = [.7;-.6;0.1;-0.7;-0.01]; % works for psi=0,45,90,135,180,225,270,315 degrees 74 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 75 | 76 | % x0 = [0.2;0.61;0.5;-0.01;0.015]; % works only for psi=45 degree 77 | 78 | % Some other tested values 79 | % x0=[0.25; 0.6; 0.4; 0.35; 0.56]; 80 | % x0 = [1.7;.6;-1;.7;.1;.5;0.08;1]; 81 | x0 = [0.1761;-0.5876;-0.7897;0.9579;0.2871]; 82 | 83 | options = optimoptions(@fsolve,'Algorithm','levenberg-marquardt'); 84 | % options = optimoptions('fsolve','Display','iter','TolX',1e-9); 85 | % options = optimoptions('fsolve','Jacobian','off') 86 | % base1=base_platform(alpha,rb); % wrt base coordinate system 87 | % top=top_platform(beta,rp); % wrt mirror coordinate system 88 | v=7/time_incre*3600+1; 89 | w=18/time_incre*3600+1; 90 | 91 | %% Plot of azimuth and elevation angles 92 | figure(1) 93 | plot(LT(v:w)/3600,elevation((v: w)),'r','LineWidth',2) 94 | hold on 95 | plot(LT(v:w)/3600,azimuth((v: w)),'b','LineWidth',2) 96 | xlabel('Time, hrs') 97 | ylabel('Angle, Degrees') 98 | % title('Variation of the leg lengths w.r.t. time') 99 | lg = legend({' Elevation',' Azimuth'},'interpreter','latex'); 100 | %set(lg,'defaulttextinterpreter','latex') 101 | grid on 102 | %% 103 | for i=1:length(v:w) 104 | zz=v-1+i; 105 | fhandle=@(x)for_mainprog_3RPS(x,OS(:,zz)); 106 | [xval(:,i),fval(:,i)] = fsolve(fhandle,x0,options); 107 | x0=xval(:,i); 108 | n1=xval(1,i);n3=xval(2,i);o3=xval(3,i);xc=xval(4,i);yc=xval(5,i); 109 | B0t(:,i)=[xc;yc;zc]; 110 | O1G=[xc;yc;zc]; % w.r.t base coordinate system (bcs) 111 | O1P_bcs=Rot'*(OP-t); % O1P_bcs = O1P described in bcs 112 | g=O1P_bcs-O1G; 113 | GP(:,i)=g/norm(g,2); 114 | GS=Rot'*OS(:,zz); % Sun vector in bcs 115 | normal=(GS+GP(:,i))/norm((GS+GP(:,i)),2); 116 | a1=normal(1,1); a2=normal(2,1); a3=normal(3,1); % wrt base coordinate system 117 | n2=-yc/rp; 118 | o1=n2; 119 | o2=n1-(2*xc/rp); 120 | R(:,:,i) = [n1 o1 a1 ; % rotation which takes mirror to base coordinate system 121 | n2 o2 a2 ; 122 | n3 o3 a3 ] ; 123 | % Trans_matr(:,:,i)=[R(:,:,i), [xc;yc;zc]; [0 0 0 1 ] ]; 124 | Trans_matr(:,:,i)= [n1 o1 a1 xc; % transformation which takes mirror to base coordinate system 125 | n2 o2 a2 yc; 126 | n3 o3 a3 zc; 127 | 0 0 0 1] ; 128 | end 129 | 130 | %% Variation of the centre of the platform 131 | figure(2) 132 | plot(B0t(1,:),B0t(2,:),'r','LineWidth',2) 133 | grid on 134 | hold on 135 | xlabel('X, m') 136 | ylabel('Y, m') 137 | xlim([-0.16 0.1]) 138 | ylim([-0.05 0.15]) 139 | 140 | aw1 = 9/time_incre*3600+1; aw2 = 12/time_incre*3600+1; aw3 = 15/time_incre*3600+1; aw4 = 17/time_incre*3600+1; 141 | txt1 = (['T = ' num2str((aw1-1)*time_incre/3600) ':00' ]); 142 | txt2 = (['T = ' num2str((aw2-1)*time_incre/3600) ':00' ]); 143 | txt3 = (['T = ' num2str((aw3-1)*time_incre/3600) ':00' ]); 144 | txt4 = (['T = ' num2str((aw4-1)*time_incre/3600) ':00' ]); 145 | % legend([p1 p2 p3],txt1,txt2,txt3,'Location','northwest', 'Interpreter', 'latex'); 146 | set(gca,'TickLabelInterpreter', 'latex'); 147 | % txt1 = (['$\psi$ = ' num2str(psi) '$^0$']); 148 | plot(B0t(1,aw1-v+1),B0t(2,aw1-v+1),'*', 'MarkerSize', 20) 149 | text(B0t(1,aw1-v+1)-0.08,B0t(2,aw1-v+1),txt1) 150 | plot(B0t(1,aw2-v+1),B0t(2,aw2-v+1),'*', 'MarkerSize', 20) 151 | text(B0t(1,aw2-v+1)-0.085,B0t(2,aw2-v+1),txt2) 152 | plot(B0t(1,aw3-v+1),B0t(2,aw3-v+1),'*', 'MarkerSize', 20) 153 | text((B0t(1,aw3-v+1))-0.018,B0t(2,aw3-v+1)+0.015,txt3) 154 | plot(B0t(1,aw4-v+1),B0t(2,aw4-v+1),'*', 'MarkerSize', 20) 155 | text((B0t(1,aw4-v+1))-0.05,B0t(2,aw4-v+1)-0.02,txt4) 156 | % zlabel('{\boldmath $ Z, m$}') 157 | %% 158 | 159 | TransmatrN=[Rot,t;[0 0 0 1]]; % transformation which takes base to global coordinate system 160 | base=TransmatrN*[O1Rs;[1 1 1]]; 161 | 162 | %% 163 | for i=1:length(Trans_matr) 164 | if i==1 165 | ad=1; 166 | else 167 | ad=i-1; 168 | end 169 | B0p1(:,:,i)=Trans_matr(:,:,i)*[GS1;1]; % w.r.t base coordinate system 170 | l1_dir(:,i)=B0p1(:,:,i)-[O1R1;1]; 171 | l1(i,:)=norm(l1_dir(:,i)); 172 | % vel1(:,i)=(l1(:,i) - l1(:,ad));%/time_incre; 173 | theta1(i,:)=180-acosd(dot(O1R1,l1_dir(1:3,i))/(norm(O1R1,2)*l1(i,:))); 174 | 175 | B0p2(:,:,i)=(Trans_matr(:,:,i)*[GS2;1]); 176 | l2_dir(:,i)=(B0p2(:,:,i)-[O1R2;1]); 177 | l2(i,:)=norm(l2_dir(:,i)); 178 | % vel2(:,i)=(l2(:,i) - l2(:,ad));%/time_incre; 179 | theta2(i,:)=180-acosd(dot(O1R2,l2_dir(1:3,i))/(norm(O1R2,2)*norm(l2_dir(:,i),2))); 180 | 181 | B0p3(:,:,i)=(Trans_matr(:,:,i)*[GS3;1]); 182 | l3_dir(:,i)=(B0p3(:,:,i)-[O1R3;1]); 183 | l3(i,:)=norm(l3_dir(:,i)); 184 | % vel3(:,i)=(l3(i,:) - l3(ad,:));%/time_incre; 185 | theta3(i,:)=180-acosd(dot(O1R3,l3_dir(1:3,i))/(norm(O1R3,2)*norm(l3_dir(:,i),2))); 186 | end 187 | %% plot of leg length 188 | figure (3) 189 | plot(LT(v:w)/3600,1000*l1(1:length(v: w)),'r--o','LineWidth',2) 190 | hold on 191 | plot(LT(v:w)/3600,1000*l2(1:length(v: w)),'g+','LineWidth',2) 192 | plot(LT(v:w)/3600,1000*l3(1:length(v: w)),'b','LineWidth',2) 193 | 194 | xlabel('Time, hrs') 195 | ylabel('Leg length, mm') 196 | % title('leg length variation wrt time') 197 | legend({'leg1','leg2', 'leg3'},'interpreter','latex') 198 | grid on 199 | hold on 200 | 201 | % stroke_l1=max(l1)-min(l1) 202 | % stroke_l2=max(l2)-min(l2) 203 | % stroke_l3=max(l3)-min(l3 204 | % Min_vel_leg1=mean(abs(vel1(2:end))) 205 | % Min_vel_leg2=mean(abs(vel2(2:end))) 206 | % Min_vel_leg3=mean(abs(vel3(2:end))) 207 | %% theta plot 208 | figure(4) 209 | plot(LT(v:w)/3600,90-theta1(1:length(v: w)),'g','LineWidth',2) 210 | hold on 211 | plot(LT(v:w)/3600,90-theta2(1:length(v: w)),'r','LineWidth',2) 212 | plot(LT(v:w)/3600,90-theta3(1:length(v: w)),'b','LineWidth',2) 213 | 214 | xlabel('Time, hrs') 215 | ylabel('Angle, Degrees') 216 | % % title('Variation of the thetas from vertical w.r.t. time') 217 | legend({'$\theta_1$','$\theta_2$','$\theta_3$'},'interpreter','latex') 218 | grid on 219 | 220 | %% Animation 221 | figure(5) 222 | % FigHandle = figure(5); 223 | % set(FigHandle, 'Position', [100, 100, 1049, 895]); 224 | % writerObj=VideoWriter('3RPS_field.avi'); 225 | % writerObj.FrameRate = 6; 226 | % open(writerObj); 227 | for i=1:length(v:w) % from 7 a.m to 6 p.m (18:00)) 228 | % for i=1:4 229 | xxx=base(1,:); % wrt global coordinate system 230 | yyy=base(2,:); % wrt global coordinate system 231 | zzz=base(3,:); % wrt global coordinate system 232 | fill3(xxx,yyy,zzz,'r') 233 | hold on 234 | OG=Rot*B0t(:,i)+t; 235 | OS1=OG+Rot*R(:,:,i)*GS1; 236 | OS2=OG+Rot*R(:,:,i)*GS2; 237 | OS3=OG+Rot*R(:,:,i)*GS3; 238 | top=[OS1 OS2 OS3]; 239 | x=top(1,:); 240 | y=top(2,:); 241 | z=top(3,:); 242 | fill3(x,y,z,'g') % top platform or mirror 243 | TRANSMATR=Rot*R(:,:,i); 244 | % %% #1 change the leg lengths also if the below plot is changed 245 | plot3([x(1) xxx(1)],[y(1) yyy(1)],[z(1) zzz(1)],'--k','LineWidth',2.5) 246 | plot3([x(2) xxx(2)],[y(2) yyy(2)],[z(2) zzz(2)],'--k','LineWidth',2.5) 247 | plot3([x(3) xxx(3)],[y(3) yyy(3)],[z(3) zzz(3)],'--k','LineWidth',2.5) 248 | % %% changing wrt global coordinate system 249 | 250 | kons=20;kon=20;zz=v-1+i; 251 | normal = plot3([OG(1,1) OG(1,1)+kon*TRANSMATR(1,3)],[OG(2,1) OG(2,1)+kon*TRANSMATR(2,3)],[OG(3,1) OG(3,1)+kon*TRANSMATR(3,3)],'r:','LineWidth',2); 252 | % proj=plot3([B0t(1,i) B0t(1,i)+kon*Trans_matr(1,3,i)],[B0t(2,i) B0t(2,i)+kon*Trans_matr(2,3,i)],[0 0],'r','LineWidth',2); 253 | 254 | sunposition=plot3([OG(1,1) OG(1,1)+kons*xs(zz)],[OG(2,1) OG(2,1)+kons*ys(zz)],[OG(3,1) OG(3,1)+kons*zs(zz)],'--k','LineWidth',2); % position of the sun at every time_incre looking from plate coordinte system 255 | plot3(OG(1,1)+kons*xs(v:zz),OG(2,1)+kons*ys(v:zz),OG(3,1)+kons*zs(v:zz),'k*'); % position of the sun at every time_incre looking from plate coordinte system 256 | % % coordintes of the receiver 257 | GP = (OP-OG)/norm((OP-OG),2); % Unit reflected ray 258 | ref_ray = plot3([OG(1,1) OG(1,1)+kon*GP(1)],[OG(2,1) OG(2,1)+kon*GP(2)],[OG(3,1) OG(3,1)+kon*GP(3)],'b','LineWidth',2); 259 | 260 | plot3([OG(1,1) a],[OG(2,1) b],[OG(3,1) c],'b','LineWidth',2); 261 | plot3([OG(1,1) a],[OG(2,1) b],[OG(3,1) c],'k*'); 262 | plot3([a a],[b b],[0 c],'b'); 263 | axis equal 264 | % xlim([10 30]); ylim([40 50]); zlim([0 20]); 265 | % xlim([-50 120]); ylim([0 30]); zlim([0 50]); 266 | axis([tx-1 tx+1 ty-1 ty+1 0 tz+3]); view(24,20) % to see the mirror 267 | % axis([-10 10 -5 5 60 70]); view(82,6) % to see the 268 | % Receiver 269 | % axis square 270 | 271 | % view(-26,34); 272 | % view(az_n(i),el_n(i)) 273 | grid on 274 | % xlabel('X, East') 275 | % ylabel('Y, North') 276 | % xlabel(' X, East','Rotation',28,'Position',[1167.5 1416 -1048.7]) 277 | % ylabel('Y, North','Rotation',-35,'Position',[1087.5 1473.8 -1053.1]) 278 | xlabel(' X, East','Rotation',-5) 279 | ylabel('Y, North','Rotation',60) 280 | zlabel(' Z, Zenith') 281 | legend([normal,sunposition,ref_ray],'Normal','Incident ray','Reflected ray') 282 | hold off 283 | M(i)=getframe(gcf); 284 | % writeVideo(writerObj,M(i)); 285 | % pause(0.1) 286 | end 287 | a 288 | % close(writerObj) 289 | 290 | % writerObj=VideoWriter('solar1.avi'); 291 | % writerObj.FrameRate = 9; 292 | % open(writerObj); 293 | %% 294 | % for i=1:length(v:w) 295 | % xn=Trans_matr(1,3,i) ; yn=Trans_matr(2,3,i) ; zn=Trans_matr(3,3,i) ; prj=[xn;yn;0]; y_axis=[0;1;0]; 296 | % ele_n(i)=atan2d(zn,norm(prj,2)); 297 | % flag = atan2d(yn,xn); 298 | % if (flag<0 && flag>=-360) 299 | % az_n(i)=rem(360+flag,360); 300 | % elseif (flag<-360) 301 | % az_n(i)=mod(360+flag,360); 302 | % else 303 | % az_n(i)=flag; 304 | % end 305 | % end 306 | % figure(5) 307 | % plot(LT(v:w)/3600,ele_n,'r*','LineWidth',4) 308 | % hold on 309 | % plot(LT(v:w)/3600,az_n,'b*','LineWidth',4) 310 | % grid on 311 | % xlabel('Time \it{t / hr}') 312 | % ylabel('Angles subtended by heliostat normal \it{\theta / (^0)}' ) 313 | % legend('Elevation','Azimuth') 314 | % 315 | % 316 | 317 | 318 | % %% velocity plot 319 | % figure (6) 320 | % plot(LT(v:w)/3600,vel1(1:length(v: w)),'r*','LineWidth',4) 321 | % hold on 322 | % plot(LT(v:w)/3600,vel2(1:length(v: w)),'b*','LineWidth',4) 323 | % plot(LT(v:w)/3600,vel3(1:length(v: w)),'g*','LineWidth',4) 324 | % % xlabel('Time \it{t / hr}') 325 | % % ylabel('Leg lengths \it{ l / m} ') 326 | % % title('Variation of the leg lengths w.r.t. time') 327 | % lgnd=legend('velocity l_1','velocity l_2','velocity l_3'); 328 | % grid on 329 | % 330 | % %% 331 | % [A1, A2, A3,B1,B2,B3]=XYZ_Euler_invert(R,theta1,theta2,theta3); 332 | % [A11, A21, A31,B11,B21,B31]=ZYZ_Euler_invert(R,theta1,theta2,theta3); 333 | [A10, A20, A30,B10,B20,B30]=ZYX_Euler_invert(R,theta1,theta2,theta3); 334 | % %% Values obtained from ADAMS 335 | % time = [0,504,1010,1510,2020,2520,3020,3530,4030,4540,5040,5540,6050,6550,7060,7560,8060,8570,9070,9580,10100,10600,11100,11600,12100,12600,13100,13600,14100,14600,15100,15600,16100,16600,17100,17600,18100,18600,19200,19700,20200,20700,21200,21700,22200,22700,23200,23700,24200,24700,25200]; 336 | % rb36_rp5_SpherRot_Leg1_X = [0.262000000000000,-0.751000000000000,-1.79000000000000,-2.84000000000000,-3.93000000000000,-5.03000000000000,-6.16000000000000,-7.30000000000000,-8.46000000000000,-9.63000000000000,-10.8000000000000,-12,-13.3000000000000,-14.5000000000000,-15.7000000000000,-17,-18.3000000000000,-19.6000000000000,-20.9000000000000,-22.2000000000000,-23.5000000000000,-24.8000000000000,-26.1000000000000,-27.5000000000000,-28.8000000000000,-30.2000000000000,-31.5000000000000,-32.9000000000000,-34.2000000000000,-35.6000000000000,-37,-38.3000000000000,-39.7000000000000,-41.1000000000000,-42.4000000000000,-43.8000000000000]; 337 | % rb36_rp5_SpherRot_Leg1_Y = [5.42000000000000,4.39000000000000,3.41000000000000,2.46000000000000,1.55000000000000,0.660000000000000,-0.192000000000000,-1.02000000000000,-1.82000000000000,-2.59000000000000,-3.33000000000000,-4.05000000000000,-4.74000000000000,-5.42000000000000,-6.07000000000000,-6.69000000000000,-7.29000000000000,-7.86000000000000,-8.42000000000000,-8.96000000000000,-9.47000000000000,-9.96000000000000,-10.4000000000000,-10.9000000000000,-11.3000000000000,-11.7000000000000,-12.1000000000000,-12.4000000000000,-12.8000000000000,-13.1000000000000,-13.4000000000000,-13.6000000000000,-13.9000000000000,-14.1000000000000,-14.3000000000000,-14.4000000000000]; 338 | % 339 | % rb36_rp5_SpherRot_Leg2_X = [6.90000000000000,6.52000000000000,6.20000000000000,5.91000000000000,5.67000000000000,5.46000000000000,5.29000000000000,5.15000000000000,5.05000000000000,4.97000000000000,4.92000000000000,4.90000000000000,4.91000000000000,4.93000000000000,4.99000000000000,5.08000000000000,5.18000000000000,5.32000000000000,5.47000000000000,5.64000000000000,5.85000000000000,6.09000000000000,6.34000000000000,6.62000000000000,6.94000000000000,7.27000000000000,7.65000000000000,8.05000000000000,8.48000000000000,8.97000000000000,9.47000000000000,10,10.6000000000000,11.3000000000000,12,12.7000000000000]; 340 | % rb36_rp5_SpherRot_Leg2_Y = [-6.91000000000000,-5.57000000000000,-4.22000000000000,-2.86000000000000,-1.48000000000000,-0.0908000000000000,1.31000000000000,2.73000000000000,4.16000000000000,5.59000000000000,7.04000000000000,8.49000000000000,9.95000000000000,11.4000000000000,12.9000000000000,14.4000000000000,15.9000000000000,17.4000000000000,18.9000000000000,20.4000000000000,21.9000000000000,23.4000000000000,24.9000000000000,26.5000000000000,28,29.5000000000000,31,32.5000000000000,34.1000000000000,35.6000000000000,37.1000000000000,38.6000000000000,40.1000000000000,41.5000000000000,43,44.4000000000000]; 341 | 342 | % rb36_rp5_SpherRot_Leg3_X = [-7.16000000000000,-5.78000000000000,-4.43000000000000,-3.08000000000000,-1.75000000000000,-0.435000000000000,0.872000000000000,2.18000000000000,3.46000000000000,4.73000000000000,6.01000000000000,7.26000000000000,8.51000000000000,9.76000000000000,11,12.2000000000000,13.5000000000000,14.7000000000000,15.9000000000000,17.1000000000000,18.3000000000000,19.6000000000000,20.8000000000000,22,23.2000000000000,24.4000000000000,25.6000000000000,26.7000000000000,27.9000000000000,29.1000000000000,30.3000000000000,31.5000000000000,32.7000000000000,33.8000000000000,35,36.2000000000000]; 343 | % rb36_rp5_SpherRot_Leg3_Y = [-6.46000000000000,-6.83000000000000,-7.23000000000000,-7.67000000000000,-8.14000000000000,-8.64000000000000,-9.17000000000000,-9.73000000000000,-10.3000000000000,-10.9000000000000,-11.5000000000000,-12.2000000000000,-12.8000000000000,-13.5000000000000,-14.2000000000000,-14.9000000000000,-15.6000000000000,-16.3000000000000,-17,-17.7000000000000,-18.5000000000000,-19.2000000000000,-19.9000000000000,-20.7000000000000,-21.4000000000000,-22.2000000000000,-22.9000000000000,-23.7000000000000,-24.4000000000000,-25.2000000000000,-25.9000000000000,-26.7000000000000,-27.4000000000000,-28.2000000000000,-28.9000000000000,-29.6000000000000]; 344 | 345 | 346 | % %% spherical joint angle plot 347 | % figure (7) % Rotaion of 1st spherical joint on the xz plane 348 | % hFig1 = figure(7); 349 | % set(hFig1, 'Position', [0 0 1800 1800]) 350 | % % mtit(hFig1,'XYZ Euler Inversion angles') 351 | % subplot(2,3,1); 352 | % plot(LT(v:w)/3600,A1(1,:),'r*','LineWidth',2) % X rot leg1 353 | % hold on 354 | % plot(LT(v:w)/3600,A1(2,:),'b*','LineWidth',2) % Y rot leg1 355 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_X ,'r','LineWidth',1 ) % Obtained from adams 356 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_Y ,'b','LineWidth',1 ) % Obtained from adams 357 | % xlabel('Time \it{t / hr}') 358 | % ylabel('Angle \it{ \theta / ^0} ') 359 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 360 | % legend('Rot abt X',' Rot abt Y') 361 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 362 | % % grid on 363 | % 364 | % % figure (8) % Rotaion of 2nd spherical joint on the y = -sqrt(3) plane 365 | % subplot(2,3,2); 366 | % plot(LT(v:w)/3600,A2(1,:),'r*','LineWidth',2) % X rot leg2 367 | % hold on 368 | % plot(LT(v:w)/3600,A2(2,:),'b*','LineWidth',2) % Y rot leg2 369 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_X ,'r','LineWidth',1 ) % Obtained from adams 370 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_Y ,'b','LineWidth',1 ) % Obtained from adams 371 | % xlabel('Time \it{t / hr}') 372 | % ylabel('Angle \it{ \theta / ^0} ') 373 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 374 | % legend('Rot abt X',' Rot abt Y') 375 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 376 | % % grid on 377 | % 378 | % % figure (9) % Rotaion of 3rd spherical joint on the y = sqrt(3) plane 379 | % subplot(2,3,3); 380 | % plot(LT(v:w)/3600,A3(1,:),'r*','LineWidth',2) % X rot leg3 381 | % hold on 382 | % plot(LT(v:w)/3600,A3(2,:),'b*','LineWidth',2) % Y rot leg3 383 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_X ,'r','LineWidth',1 ) % Obtained from adams 384 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_Y ,'b','LineWidth',1 ) % Obtained from adams 385 | % xlabel('Time \it{t / hr}') 386 | % ylabel('Angle \it{ \theta / ^0} ') 387 | % legend('Rot abt X',' Rot abt Y') 388 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 389 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 390 | % % grid on 391 | % 392 | % % figure (10) % Rotaion of 1st spherical joint on the xz plane 393 | % subplot(2,3,4); 394 | % plot(LT(v:w)/3600,B1(1,:),'r*','LineWidth',2) % X rot leg1 395 | % hold on 396 | % plot(LT(v:w)/3600,B1(2,:),'b*','LineWidth',2) % Y rot leg1 397 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_X ,'r','LineWidth',1 ) % Obtained from adams 398 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_Y ,'b','LineWidth',1 ) % Obtained from adams 399 | % xlabel('Time \it{t / hr}') 400 | % ylabel('Angle \it{ \theta / ^0} ') 401 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 402 | % legend('Rot abt X',' Rot abt Y') 403 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 404 | % % grid on 405 | % 406 | % % figure (11) % Rotaion of 2nd spherical joint on the y = -sqrt(3) plane 407 | % subplot(2,3,5); 408 | % plot(LT(v:w)/3600,B2(1,:),'r*','LineWidth',2) % X rot leg2 409 | % hold on 410 | % plot(LT(v:w)/3600,B2(2,:),'b*','LineWidth',2) % Y rot leg2 411 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_X ,'r','LineWidth',1 ) % Obtained from adams 412 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_Y ,'b','LineWidth',1 ) % Obtained from adams 413 | % xlabel('Time \it{t / hr}') 414 | % ylabel('Angle \it{ \theta / ^0} ') 415 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 416 | % legend('Rot abt X',' Rot abt Y') 417 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 418 | % % grid on 419 | % 420 | % % figure (12) % Rotaion of 3rd spherical joint on the y = sqrt(3) plane 421 | % subplot(2,3,6); 422 | % plot(LT(v:w)/3600,B3(1,:),'r*','LineWidth',2) % X rot leg3 423 | % hold on 424 | % plot(LT(v:w)/3600,B3(2,:),'b*','LineWidth',2) % Y rot leg3 425 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_X ,'r','LineWidth',1 ) % Obtained from adams 426 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_Y ,'b','LineWidth',1 ) % Obtained from adams 427 | % xlabel('Time \it{t / hr}') 428 | % ylabel('Angle \it{ \theta / ^0} ') 429 | % legend('Rot abt X',' Rot abt Y') 430 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 431 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 432 | % % grid on 433 | % % mtit(hFig1,'XYZ Euler Inversion angles') 434 | % suptitle('XYZ Euler Inversion angles') 435 | % 436 | % 437 | % %% ZYZ Euler Inversions 438 | % 439 | % figure (8) % Rotaion of 1st spherical joint on the xz plane 440 | % hFig = figure(8); 441 | % set(hFig, 'Position', [0 0 1800 1800]) 442 | % 443 | % subplot(2,3,1); 444 | % plot(LT(v:w)/3600,A11(1,:),'r*','LineWidth',2) % Z rot leg1 445 | % hold on 446 | % plot(LT(v:w)/3600,A11(2,:),'b*','LineWidth',2) % Y rot leg1 447 | % plot(LT(v:w)/3600,A11(3,:),'g*','LineWidth',2) % Z* rot leg1 448 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_X ,'r','LineWidth',1 ) % Obtained from adams 449 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_Y ,'b','LineWidth',1 ) % Obtained from adams 450 | % xlabel('Time \it{t / hr}') 451 | % ylabel('Angle \it{ \theta / ^0} ') 452 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 453 | % legend('Rot abt Z',' Rot abt Y','Rot abt Z*') 454 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 455 | % % grid on 456 | % 457 | % % figure (8) % Rotaion of 2nd spherical joint on the y = -sqrt(3) plane 458 | % subplot(2,3,2); 459 | % plot(LT(v:w)/3600,A21(1,:),'r*','LineWidth',2) % Z rot leg2 460 | % hold on 461 | % plot(LT(v:w)/3600,A21(2,:),'b*','LineWidth',2) % Y rot leg2 462 | % plot(LT(v:w)/3600,A21(3,:),'g*','LineWidth',2) % Z* rot leg2 463 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_X ,'r','LineWidth',1 ) % Obtained from adams 464 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_Y ,'b','LineWidth',1 ) % Obtained from adams 465 | % xlabel('Time \it{t / hr}') 466 | % ylabel('Angle \it{ \theta / ^0} ') 467 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 468 | % legend('Rot abt Z',' Rot abt Y', 'Rot abt Z*') 469 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 470 | % % grid on 471 | % 472 | % % figure (9) % Rotaion of 3rd spherical joint on the y = sqrt(3) plane 473 | % subplot(2,3,3); 474 | % plot(LT(v:w)/3600,A31(1,:),'r*','LineWidth',2) % Z rot leg3 475 | % hold on 476 | % plot(LT(v:w)/3600,A31(2,:),'b*','LineWidth',2) % Y rot leg3 477 | % plot(LT(v:w)/3600,A31(3,:),'g*','LineWidth',2) % Z* rot leg3 478 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_X ,'r','LineWidth',1 ) % Obtained from adams 479 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_Y ,'b','LineWidth',1 ) % Obtained from adams 480 | % xlabel('Time \it{t / hr}') 481 | % ylabel('Angle \it{ \theta / ^0} ') 482 | % legend('Rot abt Z',' Rot abt Y','Rot abt Z*') 483 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 484 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 485 | % % grid on 486 | % 487 | % % figure (10) % Rotaion of 1st spherical joint on the xz plane 488 | % subplot(2,3,4); 489 | % plot(LT(v:w)/3600,B11(1,:),'r*','LineWidth',2) % Z rot leg1 490 | % hold on 491 | % plot(LT(v:w)/3600,B11(2,:),'b*','LineWidth',2) % Y rot leg1 492 | % plot(LT(v:w)/3600,B11(3,:),'g*','LineWidth',2) % Z* rot leg1 493 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_X ,'r','LineWidth',1 ) % Obtained from adams 494 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg1_Y ,'b','LineWidth',1 ) % Obtained from adams 495 | % xlabel('Time \it{t / hr}') 496 | % ylabel('Angle \it{ \theta / ^0} ') 497 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 498 | % legend('Rot abt Z',' Rot abt Y','Rot abt Z*') 499 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 500 | % % grid on 501 | % 502 | % % figure (11) % Rotaion of 2nd spherical joint on the y = -sqrt(3) plane 503 | % subplot(2,3,5); 504 | % plot(LT(v:w)/3600,B21(1,:),'r*','LineWidth',2) % Z rot leg2 505 | % hold on 506 | % plot(LT(v:w)/3600,B21(2,:),'b*','LineWidth',2) % Y rot leg2 507 | % plot(LT(v:w)/3600,B21(3,:),'g*','LineWidth',2) % Z* rot leg2 508 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_X ,'r','LineWidth',1 ) % Obtained from adams 509 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg2_Y ,'b','LineWidth',1 ) % Obtained from adams 510 | % xlabel('Time \it{t / hr}') 511 | % ylabel('Angle \it{ \theta / ^0} ') 512 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 513 | % legend('Rot abt Z',' Rot abt Y','Rot abt Z*') 514 | % % legend('MATLAB Rot abt X',' MATLAB Rot abt Y', 'ADAMS Rot abt X',' ADAMS Rot abt Y') 515 | % % grid on 516 | % 517 | % % figure (12) % Rotaion of 3rd spherical joint on the y = sqrt(3) plane 518 | % subplot(2,3,6); 519 | % plot(LT(v:w)/3600,B31(1,:),'r*','LineWidth',2) % Z rot leg3 520 | % hold on 521 | % plot(LT(v:w)/3600,B31(2,:),'b*','LineWidth',2) % Y rot leg3 522 | % plot(LT(v:w)/3600,B31(3,:),'g*','LineWidth',2) % Z* rot leg3 523 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_X ,'r','LineWidth',1 ) % Obtained from adams 524 | % % plot(LT(v:w)/3600 , rb36_rp5_SpherRot_Leg3_Y ,'b','LineWidth',1 ) % Obtained from adams 525 | % xlabel('Time \it{t / hr}') 526 | % ylabel('Angle \it{ \theta / ^0} ') 527 | % legend('Rot abt Z',' Rot abt Y','Rot abt Z*') 528 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 529 | % suptitle('ZYZ Euler Inversion angles') 530 | 531 | %% ZYX Euler Inverstion 532 | % figure (9) % Rotaion of 1st spherical joint on the xz plane 533 | % hFig = figure(9); 534 | % set(hFig, 'Position', [0 0 1800 1800]) 535 | % 536 | % subplot(2,3,1); 537 | % plot(LT(v:w)/3600,A10(1,:),'r*','LineWidth',2) % Z rot leg1 538 | % hold on 539 | % plot(LT(v:w)/3600,A10(2,:),'b*','LineWidth',2) % Y rot leg1 540 | % plot(LT(v:w)/3600,A10(3,:),'g*','LineWidth',2) % X rot leg1 541 | % 542 | % xlabel('Time \it{t / hr}') 543 | % ylabel('Angle \it{ \theta / ^0} ') 544 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 545 | % legend('Rot abt Z',' Rot abt Y','Rot abt X') 546 | % 547 | % subplot(2,3,2); 548 | % plot(LT(v:w)/3600,A20(1,:),'r*','LineWidth',2) % Z rot leg2 549 | % hold on 550 | % plot(LT(v:w)/3600,A20(2,:),'b*','LineWidth',2) % Y rot leg2 551 | % plot(LT(v:w)/3600,A20(3,:),'g*','LineWidth',2) % X rot leg2 552 | % xlabel('Time \it{t / hr}') 553 | % ylabel('Angle \it{ \theta / ^0} ') 554 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 555 | % legend('Rot abt Z',' Rot abt Y', 'Rot abt X') 556 | % 557 | % subplot(2,3,3); 558 | % plot(LT(v:w)/3600,A30(1,:),'r*','LineWidth',2) % Z rot leg3 559 | % hold on 560 | % plot(LT(v:w)/3600,A30(2,:),'b*','LineWidth',2) % Y rot leg3 561 | % plot(LT(v:w)/3600,A30(3,:),'g*','LineWidth',2) % X rot leg3 562 | % xlabel('Time \it{t / hr}') 563 | % ylabel('Angle \it{ \theta / ^0} ') 564 | % legend('Rot abt Z',' Rot abt Y','Rot abt X') 565 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 566 | % 567 | % subplot(2,3,4); 568 | % plot(LT(v:w)/3600,B10(1,:),'r*','LineWidth',2) % Z rot leg1 569 | % hold on 570 | % plot(LT(v:w)/3600,B10(2,:),'b*','LineWidth',2) % Y rot leg1 571 | % plot(LT(v:w)/3600,B10(3,:),'g*','LineWidth',2) % X rot leg1 572 | % xlabel('Time \it{t / hr}') 573 | % ylabel('Angle \it{ \theta / ^0} ') 574 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 575 | % legend('Rot abt Z',' Rot abt Y','Rot abt X') 576 | % 577 | % subplot(2,3,5); 578 | % plot(LT(v:w)/3600,B20(1,:),'r*','LineWidth',2) % Z rot leg2 579 | % hold on 580 | % plot(LT(v:w)/3600,B20(2,:),'b*','LineWidth',2) % Y rot leg2 581 | % plot(LT(v:w)/3600,B20(3,:),'g*','LineWidth',2) % X rot leg2 582 | % xlabel('Time \it{t / hr}') 583 | % ylabel('Angle \it{ \theta / ^0} ') 584 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 585 | % legend('Rot abt Z',' Rot abt Y','Rot abt X') 586 | % 587 | % subplot(2,3,6); 588 | % plot(LT(v:w)/3600,B30(1,:),'r*','LineWidth',2) % Z rot leg3 589 | % hold on 590 | % plot(LT(v:w)/3600,B30(2,:),'b*','LineWidth',2) % Y rot leg3 591 | % plot(LT(v:w)/3600,B30(3,:),'g*','LineWidth',2) % X rot leg3 592 | % xlabel('Time \it{t / hr}') 593 | % ylabel('Angle \it{ \theta / ^0} ') 594 | % legend('Rot abt Z',' Rot abt Y','Rot abt X') 595 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 596 | % str = sprintf(' ZYX Euler Inversion angles: radius = %4.1f and \\psi = %2.1f',rad,psi); 597 | % % str = text(0.5,1,' ZYX Euler Inversion angles: radius = %4.1f and \theta psi = %2.1f',rad,psi); 598 | % suptitle(str); 599 | % suptitle('ZYX Euler Inversion angles', ) 600 | 601 | 602 | %% To make a txt file to give as input spline to ADAMS 603 | % BL_Cylin_Radius= 0.2; SL_Cylin_Leng=2.3; Sphere_Dia=BL_Cylin_Radius-0.1; 604 | % leg1 = fopen('first_leg.txt','w'); 605 | % g=0; 606 | % formatSpec = ' %4.1f \t %4.4f \n'; 607 | % for i=1:length(v:w) 608 | % fprintf(leg1,formatSpec,g,l1(i)-SL_Cylin_Leng-Sphere_Dia) 609 | % g=g+time_incre; 610 | % end 611 | % table1='first_leg.txt'; 612 | % fclose(leg1); 613 | % 614 | % leg2 = fopen('second_leg.txt','w'); 615 | % g=0; 616 | % formatSpec = ' %4.1f \t %4.4f \n'; 617 | % for i=1:length(v:w) 618 | % fprintf(leg2,formatSpec,g,l2(i)-SL_Cylin_Leng-Sphere_Dia) 619 | % g=g+time_incre; 620 | % end 621 | % table2='second_leg.txt'; 622 | % fclose(leg2); 623 | % 624 | % leg3 = fopen('third_leg.txt','w'); 625 | % g=0; 626 | % formatSpec = ' %4.1f \t %4.4f \n'; 627 | % for i=1:length(v:w) 628 | % fprintf(leg3,formatSpec,g,l3(i)-SL_Cylin_Leng-Sphere_Dia) 629 | % g=g+time_incre; 630 | % end 631 | % table3='third_leg.txt'; 632 | % fclose(leg3); 633 | %% The below figures are repeatitions of the big figure(subplots). 634 | % % This was done for the paper for solarpaces. 635 | % The solarpaces final paper Rot abt X and Rot abt Y graphs are wrong 636 | 637 | figure(15) 638 | % plot(LT(v:w)/3600,A10(1,:),'r*','LineWidth',2) % Z rot leg1 639 | hold on 640 | plot(LT(v:w)/3600,A10(2,:),'b','LineWidth',2) % Y rot leg1 641 | plot(LT(v:w)/3600,A10(3,:),'g','LineWidth',2) % X rot leg1 642 | xlabel('Time, hrs') 643 | ylabel('Angle, Degrees') 644 | % title('Variation of the spherical joint angles w.r.t. time for leg 1') 645 | legend({'Rot abt Y','Rot abt X'},'interpreter','latex') 646 | grid on 647 | % 648 | 649 | figure(16) 650 | % subplot(2,3,2); 651 | % plot(LT(v:w)/3600,A20(1,:),'r*','LineWidth',2) % Z rot leg2 652 | hold on 653 | plot(LT(v:w)/3600,A20(2,:),'b','LineWidth',2) % Y rot leg2 654 | plot(LT(v:w)/3600,A20(3,:),'g','LineWidth',2) % X rot leg2 655 | xlabel('Time, hrs') 656 | ylabel('Angle, Degrees') 657 | % title('Variation of the spherical joint angles w.r.t. time for leg 2') 658 | legend({'Rot abt Y','Rot abt X'},'interpreter','latex') 659 | grid on 660 | % 661 | 662 | figure(17) 663 | hold on 664 | plot(LT(v:w)/3600,A30(2,:),'b','LineWidth',2) % Y rot leg3 665 | plot(LT(v:w)/3600,A30(3,:),'g','LineWidth',2) % X rot leg3 666 | xlabel('Time, hrs') 667 | ylabel('Angle, Degrees') 668 | % title('Variation of the spherical joint angles w.r.t. time for leg 3') 669 | legend({'Rot abt Y','Rot abt X'},'interpreter','latex') 670 | grid on 671 | 672 | --------------------------------------------------------------------------------