155 lines
8.8 KiB
Matlab
155 lines
8.8 KiB
Matlab
clear all;
|
||
close all;
|
||
instrreset;
|
||
devNo=1; %根据实际串联的个数，修改该数量 Device Number
|
||
disp('Press Ctrl+C to stop collecting data!'); %按Ctrl+C 可以终止运行
|
||
s=serial('com3','baudrate',9600) ;fopen(s); %Open Com Port 请将COM37换成电脑识别到的COM口，波特率9600换成传感器对应的波特率
|
||
|
||
fid = fopen('20210812E.txt','wt'); %根据实际需求，可将文件“20210812E”的名称修改 According to actual needs, the name of the file "20210812E" can be modified
|
||
%文件数据格式：加速度X 加速度Y 加速度Z 角速度X 角速度Y 角速度Z 角度X 角度Y 角度Z 无效0 设备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); %每个设备的加速度有3个轴，1，2，3为1号设备，4，5，6为2号设备，......34,35,36为1号设备 There are 3 axes acceleration of each device, device 1 is 1, 2, 3, device 2 is 4, 5, 6 ... device 1 is 34, 35, 36
|
||
ww=zeros(1,devNo*3);
|
||
AA=zeros(1,devNo*3);
|
||
tt=zeros(1,1);
|
||
a=zeros(1,devNo*3); %每个设备的加速度有3个轴，1，2，3为1号设备，4，5，6为2号设备，......34,35,36为1号设备
|
||
w=zeros(1,devNo*3);
|
||
A=zeros(1,devNo*3);
|
||
Temp=zeros(1,24);
|
||
|
||
UnLockCmd=[80 06 0 105 181 136 34 161]'; %0xFF 0xAA 0x69 0x88 0xB5
|
||
CaliCmd=[80 06 0 1 0 7 148 73]'; %0xFF 0xAA 0x01 0x07 0x00
|
||
OutCaliCmd=[80 06 0 1 0 0 213 139]'; %0xFF 0xAA 0x01 0x00 0x00
|
||
SaveCmd=[80 06 0 0 0 0 132 75]'; %0xFF 0xAA 0x00 0x00 0x00
|
||
|
||
StartCali='CALI';
|
||
CalFlag=0;
|
||
|
||
D1=[ %如果修改devNo的数量，则需要往下面添加指令，目前为1个设备 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)
|
||
if CalFlag==0
|
||
CalFlag=input('请输入CALI进行校准','s');
|
||
if strcmp(CalFlag,StartCali)==1
|
||
fwrite(s,UnLockCmd(1:8),'uint8'); %解锁 Unlock
|
||
pause(1);
|
||
fwrite(s,CaliCmd(1:8),'uint8'); %解锁完成后，等待1秒，发送进入磁场校准 After unlocking, wait for 1 second and send command to enter the magnetic field calibration
|
||
disp('校准中，请分别绕3个轴慢速各旋转一圈，60秒后自动退出校准')
|
||
pause(60);
|
||
fwrite(s,UnLockCmd(1:8),'uint8'); %解锁 Unlock
|
||
pause(1);
|
||
fwrite(s,OutCaliCmd(1:8),'uint8'); %等待60秒，校准完成后，退出校准 Wait 60 seconds, after the calibration is complete, exit the calibration
|
||
pause(1);
|
||
fwrite(s,SaveCmd(1:8),'uint8'); %等待1秒，保存校准参数 Wait 1 second to save the calibration parameters
|
||
disp('校准完成，请Y轴指北，确认是否正确')
|
||
pause(1);
|
||
StartTime=cputime;
|
||
end
|
||
end
|
||
|
||
t=cputime-StartTime; %获取CPU时间减去初始时间 Get the CPU time minus the initial time
|
||
for NumCnt=1:devNo
|
||
fwrite(s,D0((NumCnt-1)*8+1:NumCnt*8),'uint8'); %串口发送读取指令 Serial sends the read command
|
||
PHead=D0((NumCnt-1)*8+1); %取ID用于判断回来的数据是否符合ID Get the ID to determine whether the returned data conforms to the ID
|
||
|
||
Head = fread(s,3,'uint8'); %先读取3个字节，判断是否符合Modbus协议头 First read 3 bytes, and then judge whether it conforms to the Modbus protocol header
|
||
|
||
if (Head(1)~=uint8(PHead))
|
||
pause(0.05);
|
||
continue; %如果不通过，则不执行以下程序，立马执行下一个读取 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 %连续读取数据24个 加速度*3 角速度*3 磁场*3 角度*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'); %低字节为无符号 The low byte is unsigned
|
||
end
|
||
if(mod(i,2)~=0)
|
||
Temp(i)=fread(s,1,'int8'); %高字节为有符号 The high byte is signed
|
||
end
|
||
end
|
||
End=fread(s,1,'uint16'); %读取最后两个字节，不需要处理 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;%取加速度X Get acceleration X
|
||
a(BuffIndex2)=single(int16(Temp(3)*256+Temp(4)))/32768*16;%取加速度Y Get acceleration Y
|
||
a(BuffIndex3)=single(int16(Temp(5)*256+Temp(6)))/32768*16;%取加速度Z Get acceleration Z
|
||
w(BuffIndex1)=single(int16(Temp(7)*256+Temp(8)))/32768*2000;%取角速度X Get angular velocity Y
|
||
w(BuffIndex2)=single(int16(Temp(9)*256+Temp(10)))/32768*2000;%取角速度Y Get angular velocity Y
|
||
w(BuffIndex3)=single(int16(Temp(11)*256+Temp(12)))/32768*2000;%取角速度Z Get angular velocity Y
|
||
A(BuffIndex1)=single(int16(Temp(19)*256+Temp(20)))/32768*180;%取角度X Get angle Y
|
||
A(BuffIndex2)=single(int16(Temp(21)*256+Temp(22)))/32768*180;%取角度Y Get angle Y
|
||
A(BuffIndex3)=single(int16(Temp(23)*256+Temp(24)))/32768*180;%取角度Z Get angle Y
|
||
end
|
||
%以上程序为读取传感器数据部分 The above program is to read part of the sensor data
|
||
|
||
%以下程序为显示及存储数据部分 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; %例如，NumCnt=5，则当前要刷新显示的为第5个设备的数据，则3*5-2=13，则第5个设备的数据存放在数组的第13~15的位置 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);%如果不需要Figure显示，则注释，可以加快程序运行时间 If don't need Figure display, then comment, which can speed up the program running time
|
||
PlotTemp2=ww(:,BuffIndex1:BuffIndex3);%如果不需要Figure显示，则注释，可以加快程序运行时间
|
||
PlotTemp3=AA(:,BuffIndex1:BuffIndex3);%如果不需要Figure显示，则注释，可以加快程序运行时间
|
||
%第一种显示方式
|
||
subplot(6,6,3*NumCnt-2);plot(tt,PlotTemp1);title(['Acc = ' num2str(a(BuffIndex1:BuffIndex3)) 'm2/s']);ylabel('m2/s'); %如果不需要Figure显示，则注释，可以加快程序运行时间
|
||
subplot(6,6,3*NumCnt-1);plot(tt,PlotTemp2);title(['Gyro = ' num2str(w(BuffIndex1:BuffIndex3)) '°/s']);ylabel('°/s'); %如果不需要Figure显示，则注释，可以加快程序运行时间
|
||
subplot(6,6,3*NumCnt-0);plot(tt,PlotTemp3);title(['Angle = ' num2str(A(BuffIndex1:BuffIndex3)) '°']);ylabel('°'); %如果不需要Figure显示，则注释，可以加快程序运行时间
|
||
%第二种显示方式
|
||
% 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)) '°/s']);ylabel('°/s');
|
||
% subplot(12,3,3*NumCnt-0);plot(tt,PlotTemp3);title(['Angle = ' num2str(A(BuffIndex1:BuffIndex3)) '°']);ylabel('°');
|
||
|
||
drawnow;
|
||
|
||
DataTxt(:,1:3)=a(:,BuffIndex1:BuffIndex3);%数据存储部分 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);%数据存储部分
|
||
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);
|