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++;  

      }  

 }