matlab机器人联合单片机仿真

来源：互联网发布：linux hexdump 编辑：程序博客网时间：2024/05/16 10:36

%这里的每个连杆的数据是和下一个坐标系之间的关系，也就是求得的是机械手末端的坐标系
L{1} =link([-pi/2    0       pi/2    0      0       0.00001 0 0 0 0 0 0 0 0 0 0 0],'sta')% 0.00001是连杆质量
L{2} =link([0       149      0       432   0     0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{3} =link([pi/2    0        pi/2     -20    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{4} =link([-pi/2    433     0        0      0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{5} =link([pi/2     0       0        0          0      0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{6} =link([0        56      0        0          0      0.000004 0 0 0 0 0 0 0 0 0 0 0],'sta')
p560m=robot(L);

c=[0 0 pi/2 0 0 0];
T1=fkine(p560m,c);
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0];
t=0:0.056:10;
T=ctraj(T1,T2,length(t));
p=ikine(p560m,T);

%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求步进电机所需脉冲
bjj=pi/180*5.625*1/64; %步进角度 pi/180是matlab1的1个度的角度值 1/64是减速比
p1= p(length(t),:)-c; %取得目标时的关节角与初始关节角之差
p2=rem(p1,pi*2) ; %对差角取余数得到一圈之内的角度
for i1=1:6 %把反转角改为统一正转
if p2(1,i1)<0
p2(1,i1)=2*pi+p2(1,i1);
end
end
mcf=round(p2/bjj);% 得到要输入的脉冲个数

%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%%%创建端口
%也可以用scom=serial(‘COM1‘,‘BaudRate‘,9600,‘Parity‘,‘none‘,‘DataBits‘,8,‘StopBits‘,1)

scom=serial('com1'); %创建串口1的设备对象scom %%%%%使用的是虚拟串口com1
scom.Terminator='LF'; %设置终止符为CR（回车符），缺省为LF（换行符）
scom.InputBufferSize=1024; %输入缓冲区为256B，缺省值为512B
scom.OutputBufferSize=1024; %输出缓冲区为256B，缺省值为512B
scom.Timeout=0.5; %Y设置一次读或写操作的最大完成时间为0.5s,缺省值为10s
scom.BaudRate=9600; %波特率
scom.Parity='none'; %无奇偶校验位
scom.StopBits=1;%%停止位
scom.ReadAsyncMode='continuous'; %(缺省方式) 读取连续接收数据自动地存入输入缓冲区
scom.BytesAvailableFcnMode='byte'; %中断触发事件为接收到的字符数达到人工设定的数目时
scom.BytesAvailableFcnCount=16; %接收缓冲区每收到16个字节时，触发回调函数
scom.BytesAvailableFcn=@instrcallback; %得到回调函数句柄
fopen(scom);%连接串口设备对象

fwrite(scom,mcf,'uint16');% 每个数据以16 位写入单片机
out=fread(scom,1,'uint8');%读取返回的检测值
kg=1;
while kg==1 %检测是否发送成功
if out(1,1)==255%说明没有发送成功
for i=1:1000;
aaaa=0;
end
fwrite(scom,mcf,'uint16');
out=fread(scom,1,'uint8')
elseif out(1,1)==254
break
end
end
%当不再使用该串口设备对象时，顺序使用3条命令，可以将所创建的串口对象对象清除，以免占用系统资源。
fclose(scom);%关闭串口设备对象
delete(scom);%删除内存中的串口设备对象
clear scom; %清除工作空间中的串口设备对象

%%fprintf(scom,'%d',fff);%写串口，发送握手信号0xFF(等价于十进制下的数值255)
% fprintf，写函数，以字符方式写
% fwrite(obj，A)，写函数，以二进制方式写，A为写入的数据，以数组形式存储
% out=fscanf(scom,'%d',[1,6])读函数，以字符方式读，以Terminator为结束符，带有超时返回功能
%out=fread(scom,4,‘uint8‘)%接收单片机发送的4个数据（8位），并存入out数组中
% fread(obj，size)，读函数，以而进制方式读，读指定字符数
% fgets，读函数，以字符方式读，包括Terminator
% fgetl，读函数，以字符方式读，忽略Terminator

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
x(i)=T(1,4,i);
y(i)=T(2,4,i);
z(i)=T(3,4,i);
end
%plot3(x,y,z)%画出运动轨迹
%plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
%drivebot(p560m)

t1=t'
%B1=[t1 F]%电机输出力矩
%B2=[t1 U]%电机控制电压
a1=p(:,1)
a2=p(:,2)
a3=p(:,3)
a4=p(:,4)
a5=p(:,5)
a6=p(:,6)
%输出到 simulink的变量 [t1,a1]
%plot(F)

=============================================================================================

==============================================================================================

单片机程序

============================================================================================

==============================================================================================

/*************************************************************************************
* 标题: 步进电机试验二 *
* *
*; 单双八拍工作方式： *
*; A-AB-B-BC-C-CD-D-DA (即一个脉冲,转 3.75 度)
**************************************************************************************/
#include "reg52.h"
#define uchar unsigned char
#define uint unsigned int
uchar Receive_Buffer[13]; //接收缓冲
uchar Buf_Index=0; //缓冲空间索引
int a1,a2,aa;
uchar code table1[4]={0x03,0x06,0x0c,0x09}; /*4节拍正转表*/
uchar code table2[4]={0x03,0x09,0x0c,0x06}; /*4节拍反转表*/
uchar code FFW[8]={0xfe,0xfc,0xfd,0xf9,0xfb,0xf3,0xf7,0xf6};//八节拍
int zj16[6];// //申请用于保存合并好的16位数据
void delay(unsigned int t);
/////////////////////// //////////////////////////////////
//void Serial_INT(); //测试
// uchar c; //测试
//数码管编码
//uchar code DSY_CODE[]={0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x00};
//Motor
sbit F1 = P1^0;
sbit F2 = P1^1;
sbit F3 = P1^2;
sbit F4 = P1^3;
////////////////////////////////////////////////////////////步进电机驱动
void motor_ffw()
{

//int i1=0;//最大脉冲
int i2=0; //当前脉
int j=0;

int kg=1; //循环结束开关
int PP1,PP2,PP3;
while(kg==1)
{
//////////////////////0000000000000

if(i2<=zj16[0]&&i2<=zj16[1])
{ PP1 = FFW[j]&0xf; //取节拍

PP2=PP1;
PP3=PP2<<4;
P0=PP1|PP3; // 给P0脚的两个电动机赋值
kg=1;

}

else if(i2<=zj16[0])
{ P0 = FFW[j]&0xf; // 给p0角的第1个电动机赋值
kg=1;
  }
else if(i2<=zj16[1])
{ PP1 = FFW[1]&0xf;
PP2=PP1;
P0=PP2<<4; // 给p0角的第2个电动机赋值
kg=1;
}
else {kg=0; }
//11111111111
if(i2<=zj16[2]&&i2<=zj16[3])
{ PP1 = FFW[j]&0xf;

PP2=PP1;
PP3=PP2<<4;
P1=PP1|PP3;
kg=1; // // 给P1脚的两个电动机赋值}

else if(i2<=zj16[2])
{ P1 = FFW[j]&0xf;
kg=1;
}
else if(i2<=zj16[3])
{ PP1 = FFW[1]&0xf;
PP2=PP1;
P1=PP2<<4;
kg=1; //// 给p1角的第二个电动机赋值
}
else {kg=0; }
//22222222222222222
if(i2<=zj16[4]&&i2<=zj16[5])
{ PP1 = FFW[j]&0xf;

PP2=PP1;
PP3=PP2<<4;
P2=PP1|PP3; // 给P2脚的两个电动机赋值
kg=1;
}

else if(i2<=zj16[4])
{ P2= FFW[j]&0xf;
kg=1;
   }
else if(i2<=zj16[5])
{ PP1= FFW[1]&0xf;
PP2=PP1;
P2=PP2<<4;// 给p2角的第二个电动机赋值
kg=1;
}
else {kg=0; }
//----------------------------
//i1++;

j++ ;
if(j>=8){ j=0;}//在8拍内转
//if(i1>=0xffff){break;}//避免无限循环
delay(10); //调节转?

}

}

/////////////////////////////////// //步进电机驱动 2222222222222 测试电机
void motor_ffw1()
{
unsigned char i;

for (i=0; i<8; i++) //一个周期转30度
{
P1 = FFW[i]&0x1f; //取数据
delay(4); //调节转速
}
}

/////////////////////////////////////////////////////////////延时
void delay(uint ms)
{
uchar i;
while(ms--) for(i=0;i<100;i++);
}
///////////////////////////////////////////////////////主程序
main()
{
uchar i=0;
uchar j2=0;
P0=0x00;
Receive_Buffer[0]=-1;
Receive_Buffer[12]=0;
SCON=0x50; // 串口模式1，允许接收
TMOD=0x20; //T1 工作模式2
TH1=0xfd; //波特率9600
TL1=0xfd;
PCON=0x00; //波特率不倍增
EA=1;EX0=1;IT0=1;
ES=1;IP=0x01;
TR1=1;
while(1)
{ //motor_ffw1(); //测试电机

// c=1;
// for(i=0;i<12;i++){ Serial_INT();}//测试整个程序

////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(Receive_Buffer[12]==-1)

{
j2=0;
for(i=0;i<6;i++)
{

a1=Receive_Buffer[j2];

a2=Receive_Buffer[j2+1];
a2<<=8;
aa=a1|a2;//合并为16位字节
zj16[i]=aa*2; //保存到数组*2是因为matlab按4拍计算，这里变为8拍
j2=j2+2;
}

motor_ffw();
Receive_Buffer[12]=0;
}
delay(4);

}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 串口接收中断函数
void Serial_INT() interrupt 4
{
uchar c; //测试
if(RI==0) return; // 测试
ES=0; //关闭串口中断
RI=0; //清接收中断标志
c=SBUF; //测试
if(Receive_Buffer[12]==0)
{

Receive_Buffer[Buf_Index]=c;
Buf_Index++;

if(Buf_Index>=12)
{ //缓存新接收每12个字符，在其后放-1 为结束标志

Receive_Buffer[12]=-1;
Buf_Index=0;
ES=1;
SBUF=0xfe; ////////////////////////////////// //返回给matlab接收成功标志 oxfe
while(TI==0); // ti=1 表示发送完毕
TI=0;
}

}
else{ //否者就返回一个oxff失败标志
ES=1;
SBUF=0xff; ///////////////////////////////// //返回给matlab接收失败标志 oxff
while(TI==0); // ti=1 表示发送完毕
TI=0;
}
ES=1;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //外部中断0 发送函数
void EX_INT0() interrupt 0
{
uchar *s="这是由8051 发送的字符串!====";
uchar i=0;
while(s[i]!='\0')
{
SBUF=s[i];
while(TI==0); // ti=1 表示发送完毕
TI=0;

i++;
}
i=0;
while(Receive_Buffer[i]!='z'&&i<12)
{
SBUF=Receive_Buffer[i];
while(TI==0); // ti=1 表示发送完毕
TI=0;

i++;
}
}

matlab机器人 联合单片机仿真

matlab机器人联合单片机仿真