matlab机器人 联合单片机仿真

%这里的每个连杆的数据是和下一个坐标系之间的关系，也就是求得的是机械手末端的坐标系  
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++;  
      }  
 }