2020年12月03日 | 单片机机械臂PWM波联合控制多路舵机按规律动作程序

简单的机械臂控制系统设计：
   没有机械臂的具体结构图，电机处仅代表机械臂关节处的运动情况。
   通过控制电机的运动来控制机械臂的具体动作。
   将货物从一个固定位置搬运到另一个固定位置，重复此操作。其中用一个压力传感器作为机械臂的启动开关，当压力传感器检测到有货物存在于货架上时机械臂开始执行动作，将货物夹起搬运到另一个位置。当货物被搬运到另一个位置的时候数码显示屏幕记录下搬运的个数，当搬运完毕后如果传感器再次检测到货架上有货物，机械臂即再一次执行刚才的动作，将货物搬运到另一个位置，同时显示屏显示的数字加一，记录下搬运的货物数量。

机械臂主要结构，各连接处用电机代替

机械臂末端示意图


五个舵机：Kpower公司的舵机，分别为：两个RC20P、一个RD13T、一个RC30P、一个RC05P3。


舵机的选择的标准：
（1）转速：转速由舵机在无负载的情况下转过60度所需要的时间。
（2）扭矩：可以理解为在舵盘上距舵机轴中心水平距离1厘米处，舵机能够带动的物品的重量。舵机扭矩的单位是千克厘米。
2.6软件设计
    （1）定义芯片引脚
    （2）定义延时函数
（3）定义中断函数
（4）定义舵机驱动函数
（5）定义模数转换器转换函数
（6）定义PWM波函数
（7）定义初始化函数
（8）通过合理的逻辑整理将要实现的功能通过逻辑运算关联起来，组合成主函数，并多次调试与修改。

单片机源程序如下:
//线性区间标度变换公式：    y=(115-15)/(243-13)*X+15kpa   
#include
#include
#include
#define uchar unsigned char
#define uint unsigned int

//ADC0832的引脚
sbit ADCS =P2^2;    //ADC0832 chip seclect
sbit ADDI =P2^4;    //ADC0832 k in
sbit ADDO =P2^4;    //ADC0832 k out
sbit ADCLK =P2^3;   //ADC0832 clock signal

sbit PWM0 = P3^0;
sbit PWM1 = P3^1;
sbit PWM2 = P3^2;
sbit PWM3 = P3^3;
sbit PWM4 = P3^4;

sbit PWM5 = P1^5;
sbit PWM6 = P1^6;
sbit PWM7 = P1^7;

sbit ADD = P3^4;
sbit B3 = P3^5;

uint t_up0 = 1500;                //舵机PWM高电平时间 1000~2000表示1ms到2ms
uint t_up1 = 1500;
uint t_up2 = 1500;
uint t_up3 = 1500;
uint t_up4 = 1500;

uint t0_h;
uint t0_l;
unsigned int D1,D2,D3,D4;
uint Flag=0;



uchar t0_flag = 0;
uint num_max = 65535;        //
uint t_change = 63036;  //换路周期2.5ms

uchar error0 = 45;
uchar error1 = 45;
uchar error2 = 52;
uchar error3 = 52;
uchar error4 = 57;
uchar error5 = 57;
uchar error6 = 63;
uchar error7 = 63;
uchar error8 = 70;
uchar error9 = 70;


unsigned char dispbitcode[8]={0xf7,0xfb,0xfd,0xfe,0xef,0xdf,0xbf,0x7f};  //位扫描
unsigned char dispcode[11]={0xC0,0xF9,0xA4,0xB0,0x99,0x92,0x82,0xF8,0x80,0x90,0xff};  //共阳数码管字段码
unsigned char dispbuf[4]; //显示数码管的数组
unsigned int temp;
unsigned char getdata; //获取ADC转换回来的值

void delayms(uint ms)
{      
        unsigned char a,b,c;
        while(ms--)
        {               
    for(c=1;c>0;c--)
        for(b=142;b>0;b--)
            for(a=2;a>0;a--);
        }
}
void timer_init()
{
        EA = 1;
        ET0 = 1;
        PT0 = 1;
        TMOD = 0x11;
        TH0 = (65536 - t_up0)/256;
        TL0 = (65536 - t_up0)%256;
}

void timer0() interrupt 1
{      
        if(t0_flag == 0)
        {
                PWM0 = 1;
                TH0 = (num_max - t_up0 + error0+50*2)/256; //是为了抵消执行语句花的时间
                TL0 = (num_max - t_up0 + error0+125)%256;
                t0_flag = 1;
        }
        else if(t0_flag == 1)
        {
                PWM0 = 0;
                TH0 = (t_change + t_up0 +error1+50*2)/256;
                TL0 = (t_change + t_up0 +error1+125)%256;
                t0_flag = 2;
        }
        else if(t0_flag == 2)
        {
                PWM1 = 1;
                TH0 = (num_max - t_up1 + error2+50*2)/256; //是为了抵消执行语句花的时间
                TL0 = (num_max - t_up1 + error2+125)%256;      
                t0_flag = 3;
        }
        else if(t0_flag == 3)
        {
                PWM1 = 0;
                TH0 = (t_change + t_up1 +error3+50*2)/256;
                TL0 = (t_change + t_up1 +error3+125)%256;      
                t0_flag = 4;
        }
        else if(t0_flag == 4)
        {
                PWM2 = 1;
                TH0 = (num_max - t_up2 + error4+50*2)/256; //是为了抵消执行语句花的时间
                TL0 = (num_max - t_up2 + error4+129)%256;      
                t0_flag = 5;
        }
        else if(t0_flag == 5)
        {
                PWM2 = 0;
                TH0 = (t_change + t_up2 +error5+50*2)/256;
                TL0 = (t_change + t_up2 +error5+129)%256;
               
                t0_flag = 6;
        }
        else if(t0_flag == 6)
        {
                PWM3 = 1;
                TH0 = (num_max - t_up3 + error6+50*2)/256; //是为了抵消执行语句花的时间
                TL0 = (num_max - t_up3 + error6+129+1)%256;
               
                t0_flag = 7;
        }
        else if(t0_flag == 7)
        {
                PWM3 = 0;
                TH0 = (t_change + t_up3 +error7+50*2)/256;
                TL0 = (t_change + t_up3 +error7+129+1)%256;      
                t0_flag = 8;
        }
        else if(t0_flag == 8)
        {
                PWM4 = 1;
                  TH0 = (num_max - t_up4 + error8+50*2)/256; //是为了抵消执行语句花的时间
                  TL0 = (num_max - t_up4 + error8+129+1)%256;      
                t0_flag = 9;
        }
        else if(t0_flag == 9)
        {
                PWM4 = 0;
                TH0 = (t_change + t_up4 +error9+50*2+10000)/256;
                TL0 = (t_change + t_up4 +error9+129+1)%256;      
                t0_flag = 0;
        }
      
}

//延时函数
void delay_1ms(void)  //12mhz delay 1.01ms
{
   unsigned char x,y;   
   x=3;
   while(x--)
  {
       y=40;
       while(y--);
    }
}

//显示函数
void display(void)                          //数码管显示函数
{
  char k;
  for(k=0;k<4;k++)
  {

  P1 = dispbitcode[k];
  P0 = dispcode[dispbuf[k]];        //显示压力传感器读取到的数值，一共四个数
//  if(k==1)                                                  //加上数码管的dp小数点
//          P0&=0x7f;                                        //p0相与后赋予新的值
    delay_1ms();      

  }
}

/************
读ADC0832函数
************/

//采集并返回
unsigned int Adc0832(unsigned char channel)     //AD转换，返回结果
{
    unsigned char i=0;
    unsigned char j;
    unsigned int dat=0;
    unsigned char ndat=0;

    if(channel==0)channel=2;
    if(channel==1)channel=3;
    ADDI=1;
    _nop_();
    _nop_();
    ADCS=0;//拉低CS端
    _nop_();
    _nop_();
    ADCLK=1;//拉高CLK端
    _nop_();
    _nop_();
    ADCLK=0;//拉低CLK端,形成下降沿1
    _nop_();
    _nop_();
    ADCLK=1;//拉高CLK端
    ADDI=channel&0x1;
    _nop_();
    _nop_();
    ADCLK=0;//拉低CLK端,形成下降沿2
    _nop_();
    _nop_();
    ADCLK=1;//拉高CLK端
    ADDI=(channel>>1)&0x1;
    _nop_();
    _nop_();
    ADCLK=0;//拉低CLK端,形成下降沿3
    ADDI=1;//控制命令结束
    _nop_();
    _nop_();
    dat=0;
    for(i=0;i<8;i++)
    {
        dat|=ADDO;//收数据
        ADCLK=1;
        _nop_();
        _nop_();
        ADCLK=0;//形成一次时钟脉冲
        _nop_();
        _nop_();
        dat<<=1;
        if(i==7)dat|=ADDO;
    }  
    for(i=0;i<8;i++)
    {
        j=0;
        j=j|ADDO;//收数据
        ADCLK=1;
        _nop_();
        _nop_();
        ADCLK=0;//形成一次时钟脉冲
        _nop_();
        _nop_();
        j=j<<7;
        ndat=ndat|j;
        if(i<7)ndat>>=1;
    }
    ADCS=1;//拉低CS端
    ADCLK=0;//拉低CLK端
    ADDO=1;//拉高数据端,回到初始状态
    dat<<=8;
    dat|=ndat;
    return(dat);            //return ad k
}



void count(void)   //计算得到D1,D2,D3,D4
{
        unsigned int OverCounter = 0;
        bit OverFlg = 0;
        unsigned int temp,ppress = 0;
        float  press;

        getdata=Adc0832(0);
        if(14        {                           
                int vary=getdata;                                  //y=(115-15)/(243-13)*X+15kpa                              
                press=((10.0/23.0)*vary)+9.3;          //测试时补偿值为9.3                                                                                                         
                temp=(int)(press*10);                  //放大10倍，便于后面的计算
               if(temp != ppress)
               {
                          ppress = temp;
                       OverFlg = 1;
                    }                                                                                          
                        D1=temp/1000;                                     //取压力值百位
                        D2=(temp%1000)/100;                            //取压力值十位
                        D3=((temp%1000)%100)/10;            //取压力值个位
                        D4=((temp%1000)%100)%10;                //取压力值十分位
                }            
}


void KEY(void)  //搬运东西电机转动程序
{      
                if(Flag<100)                              
                {
                        if(t_up0 <= 1950&&Flag<=30)
                        {
                                t_up0 = t_up0 + 5;
                                delayms(50);
                                Flag++;
                        }
                        if(t_up0 <= 1930&&(Flag>30&&Flag<=50))
                        {
                                t_up1 = t_up1 + 5;
                                delayms(50);
                                Flag++;
                        }
                        if(t_up0 <= 1930&&(Flag>50&&Flag<=70))
                        {
                                t_up2 = t_up2 + 5;
                                delayms(50);
                                Flag++;
                        }
                        if(t_up0 <= 1930&&(Flag>70&&Flag<=80))
                        {
                                t_up3 = t_up3 + 5;
                                delayms(50);
                                Flag++;
                        }
                        if(t_up0 <= 1950&&(Flag>80&&Flag<=100))
                        {
                                t_up4 = t_up4 + 5;
                                delayms(50);
                                Flag++;
                        }                                                      
                }
}

void KEY0(void)
{
                        if(t_up0 <= 1950&&Flag>=80)
                        {
                                t_up4 = t_up4 - 5;
                                delayms(50);
                                Flag--;
                        }

                        if(t_up0 <= 1930&&(Flag>=70&&Flag<80))
                        {
                                t_up3 = t_up3 - 5;
                                delayms(50);
                                Flag--;
                        }
                        if(t_up0 <= 1930&&(Flag>=50&&Flag<70))
                        {
                                t_up2 = t_up2 - 5;
                                delayms(50);
                                Flag--;
                        }
                        if(t_up0 <= 1930&&(Flag>=30&&Flag<50))
                        {
                                t_up1 = t_up1 - 5;
                                delayms(50);
                                Flag--;
                        }

                        if(t_up0 <= 1950&&(Flag>=0&&Flag<30))
                        {
                                t_up0 = t_up0 - 5;
                                delayms(50);
                                Flag--;
                        }

}

void main(void)
{
        uint a=1,b=0,c=0,d=0;
        uint flag=0,F=1,num,i=1;
        timer_init();
        TR0 = 1;

        while(1)
        {
        count();
        while(D2^F)                                                   //压力值出现变化
        {
                if(D2<2)
                {
                        while(D2^F)
                        {
                                count();
                                 if(D2>=2)
                                {
                                        flag=1;
                                        i=1;
                                        num++;
                                        if(num>0)
                                        a++;
                                        if(num==10)
                                        a=0;
                                        if(num>=10&&num<20)
                                        b=1;
                                        if(num>=1000)
                                        d++;

                                        while(Flag>0)
                                        {
                                        KEY0();
                                        }        
                                        F=D2;
                                }
                        }
                }                                                                                                                                                                                                                                                                 
                else
                F=D2;                                                                                         
        }

        dispbuf[0]=a;  //个位                                                         
        dispbuf[1]=b;  //十位
        dispbuf[2]=c;  //百位
        dispbuf[3]=d;  //千位

        if(D2>=2)
        {
                flag=1;
                i=1;
        }
        if(flag==1&&i==1)
        {
                KEY();
        }
        if(Flag==100)
        display();
        }                    
}