Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
agrobot_base/Exemplos/WitStandardModbus_WT901C485.../Matlab/DataReceive.m

129 lines
7.5 KiB
Mathematica
Raw Permalink Normal View History

iniciado revisao do fluxo de movimentacao autonoma
2024-06-19 19:52:02 +00:00
clear all;
close all;
instrreset;
devNo=1; %<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ʴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸ĸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Device Number
disp('Press Ctrl+C to stop collecting data!'); %<EFBFBD><EFBFBD>Ctrl+C <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
s=serial('com3','baudrate',9600) ;fopen(s); %Open Com Port <EFBFBD>뽫COM37<EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD>𵽵<EFBFBD>COM<EFBFBD>ڣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9600<EFBFBD><EFBFBD><EFBFBD>ɴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
fid = fopen('20210812E.txt','wt'); %<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󣬿ɽ<EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>20210812E<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD> According to actual needs, the name of the file "20210812E" can be modified
%<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>X <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Y <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Z <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>X <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Y <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Z <EFBFBD>Ƕ<EFBFBD>X <EFBFBD>Ƕ<EFBFBD>Y <EFBFBD>Ƕ<EFBFBD>Z <EFBFBD><EFBFBD>Ч0 <EFBFBD>豸ID File Data Format: Acceleration X, Acceleration Y, Acceleration Z, Angular Velocity X, Angular Velocity Y, Angular Velocity Z, Angle X, Angle Y, Angle Z, Invalid 0, Device ID
DataTxt=zeros(1,10);
f = 20;
NumCnt=1;
BuffIndex1=0;
BuffIndex2=0;
BuffIndex3=0;
DisplayIndex=1;
aa=zeros(1,devNo*3); %ÿ<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD>ļ<EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>ᣬ1<EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>3Ϊ1<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>4<EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>6Ϊ2<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>......34,35,36Ϊ12<EFBFBD><EFBFBD><EFBFBD>豸 There are 3 axes acceleration of each device, device 1 is 1, 2, 3, device 2 is 4, 5, 6 ... device 12 is 34, 35, 36
ww=zeros(1,devNo*3);
AA=zeros(1,devNo*3);
tt=zeros(1,1);
a=zeros(1,devNo*3); %ÿ<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD>ļ<EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>ᣬ1<EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>3Ϊ1<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>4<EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>6Ϊ2<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>......34,35,36Ϊ12<EFBFBD><EFBFBD><EFBFBD>豸
w=zeros(1,devNo*3);
A=zeros(1,devNo*3);
Temp=zeros(1,24);
D1=[ %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD>devNo<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD>ĿǰΪ1<EFBFBD><EFBFBD><EFBFBD>豸 If modify the number of devNo, need to add instructions below, currently 1 devices
80 3 0 52 0 12 9 128 %0x50 0x03 0x00 0x34 0x00 0x0C 0x09 0x80
% 81 3 0 52 0 12 8 81 %0x51 0x03 0x00 0x34 0x00 0x0C 0x08 0x51
% 82 3 0 52 0 12 8 98 %0x52 0x03 0x00 0x34 0x00 0x0C 0x08 0x62
% 83 3 0 52 0 12 9 179 %0x53 0x03 0x00 0x34 0x00 0x0C 0x09 0xB3
% 84 3 0 52 0 12 8 4 %0x54 0x03 0x00 0x34 0x00 0x0C 0x08 0x04
% 85 3 0 52 0 12 9 213 %0x55 0x03 0x00 0x34 0x00 0x0C 0x09 0xD5
% 86 3 0 52 0 12 9 230 %0x56 0x03 0x00 0x34 0x00 0x0C 0x09 0xE6
% 87 3 0 52 0 12 8 55 %0x57 0x03 0x00 0x34 0x00 0x0C 0x08 0x37
% 88 3 0 52 0 12 8 200 %0x58 0x03 0x00 0x34 0x00 0x0C 0x08 0xC8
% 89 3 0 52 0 12 9 25 %0x59 0x03 0x00 0x34 0x00 0x0C 0x09 0x19
% 90 3 0 52 0 12 9 42 %0x5A 0x03 0x00 0x34 0x00 0x0C 0x09 0x2A
% 91 3 0 52 0 12 8 251 %0x5B 0x03 0x00 0x34 0x00 0x0C 0x08 0xFB
];
D0 = D1';
StartTime=cputime;
while(1)
t=cputime-StartTime; %<EFBFBD><EFBFBD>ȡCPUʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><EFBFBD>ʼʱ<EFBFBD><EFBFBD> Get the CPU time minus the initial time
for NumCnt=1:devNo
fwrite(s,D0((NumCnt-1)*8+1:NumCnt*8),'uint8'); %<EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD><EFBFBD>Ͷ<EFBFBD>ȡָ<EFBFBD><EFBFBD> Serial sends the read command
PHead=D0((NumCnt-1)*8+1); %ȡID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ID Get the ID to determine whether the returned data conforms to the ID
Head = fread(s,3,'uint8'); %<EFBFBD>ȶ<EFBFBD>ȡ3<EFBFBD><EFBFBD><EFBFBD>ֽڣ<EFBFBD><EFBFBD>ж<EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ModbusЭ<EFBFBD><EFBFBD>ͷ First read 3 bytes, and then judge whether it conforms to the Modbus protocol header
if (Head(1)~=uint8(PHead))
pause(0.05);
continue; %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>³<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ If it does not pass, the following procedure is not executed, and the next read is executed immediately
end
if (Head(2)~=uint8(3))
pause(0.05);
continue;
end
if (Head(3)~=uint8(24))
pause(0.05);
continue;
end
for i=1:24 %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>24<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>*3 <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>*3 <EFBFBD>ų<EFBFBD>*3 <EFBFBD>Ƕ<EFBFBD>*3 Read 24 data continuously, acceleration*3, angular velocity*3, magnetic field*3, angle*3
if(mod(i,2)==0)
Temp(i)=fread(s,1,'uint8'); %<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>Ϊ<EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD> The low byte is unsigned
end
if(mod(i,2)~=0)
Temp(i)=fread(s,1,'int8'); %<EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD>Ϊ<EFBFBD>з<EFBFBD><EFBFBD><EFBFBD> The high byte is signed
end
end
End=fread(s,1,'uint16'); %<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽڣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Read the last two bytes, no processing required
BuffIndex1=3*(NumCnt)-2;
BuffIndex2=3*(NumCnt)-1;
BuffIndex3=3*(NumCnt);
a(BuffIndex1)=single(int16(Temp(1)*256+Temp(2)))/32768*16;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>X Get acceleration X
a(BuffIndex2)=single(int16(Temp(3)*256+Temp(4)))/32768*16;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Y Get acceleration Y
a(BuffIndex3)=single(int16(Temp(5)*256+Temp(6)))/32768*16;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Z Get acceleration Z
w(BuffIndex1)=single(int16(Temp(7)*256+Temp(8)))/32768*2000;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>X Get angular velocity X
w(BuffIndex2)=single(int16(Temp(9)*256+Temp(10)))/32768*2000;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Y Get angular velocity Y
w(BuffIndex3)=single(int16(Temp(11)*256+Temp(12)))/32768*2000;%ȡ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Z Get angular velocity Z
A(BuffIndex1)=single(int16(Temp(19)*256+Temp(20)))/32768*180;%ȡ<EFBFBD>Ƕ<EFBFBD>X Get angle X
A(BuffIndex2)=single(int16(Temp(21)*256+Temp(22)))/32768*180;%ȡ<EFBFBD>Ƕ<EFBFBD>Y Get angle Y
A(BuffIndex3)=single(int16(Temp(23)*256+Temp(24)))/32768*180;%ȡ<EFBFBD>Ƕ<EFBFBD>Z Get angle Z
end
%<EFBFBD><EFBFBD><EFBFBD>ϳ<EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><EFBFBD><EFBFBD> The above program is to read part of the sensor data
%<EFBFBD><EFBFBD><EFBFBD>³<EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><EFBFBD><EFBFBD> The following program is to display and store part of data
aa = [aa;a];
ww = [ww;w];
AA = [AA;A];
tt = [tt;t];
for NumCnt=1:devNo
BuffIndex1=3*NumCnt-2; %<EFBFBD><EFBFBD><EFBFBD>磬NumCnt=5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰҪˢ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>5<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><EFBFBD><EFBFBD>3*5-2=13<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>13~15<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> For example, NumCnt=5, which means that the current data to be refreshed and displayed is the data of the fifth device, that is, 3*5-2=13, so the data of the fifth device is stored in the 13th~15th position of the array
BuffIndex2=3*NumCnt-1;
BuffIndex3=3*NumCnt;
PlotTemp1=aa(:,BuffIndex1:BuffIndex3);%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD> If don't need Figure display, then comment, which can speed up the program running time
PlotTemp2=ww(:,BuffIndex1:BuffIndex3);%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
PlotTemp3=AA(:,BuffIndex1:BuffIndex3);%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
%<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ʽ
subplot(6,6,3*NumCnt-2);plot(tt,PlotTemp1);title(['Acc = ' num2str(a(BuffIndex1:BuffIndex3)) 'm2/s']);ylabel('m2/s'); %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
subplot(6,6,3*NumCnt-1);plot(tt,PlotTemp2);title(['Gyro = ' num2str(w(BuffIndex1:BuffIndex3)) '<EFBFBD><EFBFBD>/s']);ylabel('<EFBFBD><EFBFBD>/s'); %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
subplot(6,6,3*NumCnt-0);plot(tt,PlotTemp3);title(['Angle = ' num2str(A(BuffIndex1:BuffIndex3)) '<EFBFBD><EFBFBD>']);ylabel('<EFBFBD><EFBFBD>'); %<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҪFigure<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
%<EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ʽ
% subplot(12,3,3*NumCnt-2);plot(tt,PlotTemp1);title(['Acc = ' num2str(a(BuffIndex1:BuffIndex3)) 'm2/s']);ylabel('m2/s');
% subplot(12,3,3*NumCnt-1);plot(tt,PlotTemp2);title(['Gyro = ' num2str(w(BuffIndex1:BuffIndex3)) '<EFBFBD><EFBFBD>/s']);ylabel('<EFBFBD><EFBFBD>/s');
% subplot(12,3,3*NumCnt-0);plot(tt,PlotTemp3);title(['Angle = ' num2str(A(BuffIndex1:BuffIndex3)) '<EFBFBD><EFBFBD>']);ylabel('<EFBFBD><EFBFBD>');
drawnow;
DataTxt(:,1:3)=a(:,BuffIndex1:BuffIndex3);%<EFBFBD><EFBFBD><EFBFBD>ݴ洢<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Data storage part
DataTxt(:,4:6)=w(:,BuffIndex1:BuffIndex3);
DataTxt(:,7:9)=A(:,BuffIndex1:BuffIndex3);
fprintf(fid,'%g\t',DataTxt');
PHead=D0((NumCnt-1)*8+1);
fprintf(fid,'%g\n',PHead);%<EFBFBD><EFBFBD><EFBFBD>ݴ洢<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
end
if (size(aa,1)>5*f)%clear history data
aa = aa(f:5*f,:);
ww = ww(f:5*f,:);
AA = AA(f:5*f,:);
tt = tt(f:5*f,:);
end
end
fclose(s);