首页 › 项目展示 › 6轴体感式机械臂

jzengming 发表于 2017-3-12 12:43

6轴体感式机械臂

本帖最后由 jzengming 于 2017-3-12 12:43 编辑

学校的项目本来想着自己搞着玩来着，结果搞着搞着没动力了，今天发到这里来。大家共同监督共同进步吧。
首先是硬件部分由于硬件部分比较简单我就不多说了直接上图了
http://www.arduino.cn/forum.php?mod=image&aid=30775&size=300x300&key=0b15353e1c11e6a5&nocache=yes&type=fixnonehttp://www.arduino.cn/forum.php?mod=image&aid=30774&size=300x300&key=388078d0a5599c20&nocache=yes&type=fixnonehttp://www.arduino.cn/forum.php?mod=image&aid=30773&size=300x300&key=957b6bd93ce87da8&nocache=yes&type=fixnone

最初的想法是利用上位机采集手臂的动作，然后将数据发送至下位机，下位机进行数据的处理，以及最终机械手臂的动作。因为用到了两个9轴传感器所以上位机用的是2560毕竟它有4个物理串口。这样我就可以直接读取串口的值进行传输就可以了，开始时由于不会改写2.4g通讯程序 只能采取简单的取不同范围值进行传输 简单但是有效，稍后我会更新使用数据包传输的方法希望之后还能有时间。
上位机程序


//GY953模块通信子程序定义部分
#include <Wire.h>
int YPR;
unsigned char Re_buf,counter=0;
unsigned char sign=0;
int YPR2;
unsigned char Re_buf2,counter2=0;
unsigned char sign2=0;
int led = 13;
//发送子程序段定义部分
#include <Wire.h>
#include <SPI.h>
#include <Mirf.h>
#include <nRF24L01.h>
#include <MirfHardwareSpiDriver.h>
int adata =0;
//初始化设置部分
void setup()
{
//初始化波特率
Serial.begin(115200);
Serial3.begin(115200);
Serial2.begin(115200);

// 稳定波特率延时
delay(2000);   
// 初始化GY953-1,连续输出模式
Serial3.write(0XA5);
Serial3.write(0X45);   
Serial3.write(0XEA);   
//初始化GY953-2,连续输出模式
Serial2.write(0XA5);
Serial2.write(0X45);   
Serial2.write(0XEA);   
//初始化2.4G无线通信模块
//中断部分

attachInterrupt(0, coor_loop0, LOW);
attachInterrupt(1, coor_loop1, LOW);
attachInterrupt(2, coor_loop2, LOW);
    Mirf.cePin = 48;         //设置CE引脚为D9
    Mirf.csnPin = 53;      //设置CE引脚为D10
    Mirf.spi = &MirfHardwareSpi;
    Mirf.init();//初始化nRF24L01               

   //设置接收标识符"Sen01"
    Mirf.setRADDR((byte *)"Sen01");
    //设置一次收发的字节数，这里发一个整数，写sizeof(unsigned int)，实际等于2字节
    Mirf.payload = sizeof(unsigned int);
    //发送通道，可以填0~127，收发必须一致。
    Mirf.channel = 3;
    Mirf.config();
      //注意一个Arduino写Sender.ino，另一个写Receiver.ino。
      //这里标识写入了Sender.ino
    Serial.println("I'm Sender...");
}
//-------------------------------------------------------------
void loop() {



if(sign)
{// Serial.println("Y");
   sign=0;
   if(Re_buf==0x5A&&Re_buf==0x5A )      //检查帧头，帧尾
   {             
            YPR=(Re_buf<<8|Re_buf)/100;   //合成数据，去掉小数点后2位
            sendadata( YPR);
            YPR=(Re_buf<<8|Re_buf)/100;
             sendadata( YPR+361);
            YPR=(Re_buf<<8|Re_buf)/100;
             sendadata( YPR+722);
         Serial.print("YPR:\t");
         Serial.print(YPR, DEC); Serial.print("\t"); //显示航向
         Serial.print(YPR, DEC); Serial.print("\t"); //显示俯仰角
         Serial.print(YPR, DEC);                  //显示横滚角      

         delay(10);         
   }
}


if(sign2)
{// Serial.println("Y");
   sign2=0;
   if(Re_buf2==0x5A&&Re_buf2==0x5A )      //检查帧头，帧尾
   {             
            YPR2=(Re_buf2<<8|Re_buf2)/100;   //合成数据，去掉小数点后2位
            sendadata( YPR2-361);
            YPR2=(Re_buf2<<8|Re_buf2)/100;
             sendadata( YPR2-722);
            YPR2=(Re_buf2<<8|Re_buf2)/100;
             sendadata( YPR2-1083);
         Serial.print("YP:\t");
         Serial.print(YPR2, DEC); Serial.print("\t"); //显示航向
         Serial.print(YPR2, DEC); Serial.print("\t"); //显示俯仰角
         Serial.println(YPR2, DEC);                  //显示横滚角               
         delay(10);         
   }
}

}
//中断原点设置
void coor_loop0()
{
sendadata(2001);
delay(10);

}

void coor_loop1()
{
sendadata(2002);
delay(10);

}
void coor_loop2()
{
sendadata(2003);
delay(10);

}
//数据传输子程序
void sendadata(int x)
{
    adata = x;//读取A0值到adata

    //由于nRF24L01只能以byte单字节数组形式发送Mirf.payload个数据，
    //所以必须将所有需要传输的数据拆成byte。
    //下面定义byte数组，存放待发数据，因为Mirf.payload = sizeof(unsigned int);
    //实际下面等于byte data;
    byte data;

    //adata是unsigned int双字节数据，必须拆开。
    //将adata高低八位拆分：
    data = adata & 0xFF;                //低八位给data，
    data = adata >> 8;                //高八位给data。

    //设置向"serv1"发送数据
    Mirf.setTADDR((byte *)"Rec01");
    Mirf.send(data);
    //while死循环等待发送完毕，才能进行下一步操作。
    while(Mirf.isSending()) {}
    delay(20);
}
//---------------------------------------------------------------

void serialEvent3() {
while (Serial3.available()) {   
    // Serial.println("YPR");
    Re_buf=(unsigned char)Serial3.read();
    if(counter==0&&Re_buf!=0x5A) return;      // 检查帧头         
    counter++;      
    if(counter==11)                //接收到数据
    {   
       counter=0;               //重新赋值，准备下一帧数据的接收
       sign=1;
    }      
}
}
void serialEvent2() {
while (Serial2.available()) {   
    // Serial.println("YPR");
    Re_buf2=(unsigned char)Serial2.read();
    if(counter2==0&&Re_buf2!=0x5A) return;      // 检查帧头         
    counter2++;      
    if(counter2==11)                //接收到数据
    {   
       counter2=0;               //重新赋值，准备下一帧数据的接收
       sign2=1;
    }      
}
}

下位机程序
当然这个程序只写到数据处理部分具体动作算法部分还没写:$:$/*
nRF24L01 Arduino Receiver接收端

Ansifa
2015/3/7

引脚接法：
nRF24L01   Arduino UNO
VCC <-> 3.3V
GND <-> GND
CE<-> D9
CSN <-> D10
MOSI<-> D11
MISO<-> D12
SCK <-> D13
IRQ <-> 不接
*/
#include <Servo.h>
#include <SPI.h>
#include <Mirf.h>
#include <nRF24L01.h>
#include <MirfHardwareSpiDriver.h>
#include <Servo.h>
    //定义一个变量adata存储最终结果,oldadata存储旧结果，防止相同结果刷屏。
int adata = 0, oldadata = 0;
int YPR;
int oldYPR;
int newadata;
int coor_num;
int counter=0;

int jd1;
int jd234;
int jd5;
int jd6;
boolean sign =true ;
Servo myservo1;// create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;
intstand_adata_x_hand , stand_adata_y_hand , stand_adata_z_hand ,
   stand_adata_x_arm, stand_adata_y_arm ,stand_adata_z_arm ;
intold_stand_adata_x_hand , old_stand_adata_y_hand , old_stand_adata_z_hand ,
   old_stand_adata_x_arm, old_stand_adata_y_arm ,old_stand_adata_z_arm ;

void setup()
{
    Serial.begin(115200);
   myservo1.attach(3);// attaches the servo on pin 9 to the servo object
// myservo1.write(0);      
   myservo2.attach(4);
//myservo2.write(180);
   myservo3.attach(5);
// myservo3.write(180);
   myservo4.attach(6);
//myservo4.write(90);
   myservo5.attach(7);
//   myservo5.write(0);
   myservo6.attach(8);
//   myservo6.write(90);
    //---------初始化部分，不可随时修改---------
    Mirf.cePin = 9;   //设置CE引脚为D9
    Mirf.csnPin = 10;   //设置CE引脚为D10
    Mirf.spi = &MirfHardwareSpi;
    Mirf.init();//初始化nRF24L01

    //---------配置部分，可以随时修改---------
    //设置接收标识符"Rev01"
    Mirf.setRADDR((byte *)"Rec01");
    //设置一次收发的字节数，这里发一个整数，
    //写sizeof(unsigned int)，实际等于2字节
    Mirf.payload = sizeof(unsigned int);
    //发送通道，可以填0~128，收发必须一致。
    Mirf.channel = 3;
    Mirf.config();

    //注意一个Arduino写Sender.ino，另一个写Receiver.ino。
    //这里用来辨别写入了Receiver.ino程序

}

void loop()
{



adatae();



   Serial.print("   stand_x =");
   Serial.print( stand_adata_x_hand);
   Serial.print("   stand_y =");
   Serial.print( stand_adata_y_hand);
   Serial.print("   stand_z =");
   Serial.print( stand_adata_z_hand);
   Serial.print("   stand_x_arm =");
   Serial.print( stand_adata_x_arm);
   Serial.print("   stand_y_arm = ");
   Serial.print( stand_adata_y_arm);
   Serial.print("   stand_z_arm = ");
   Serial.println( stand_adata_z_arm);

   }   



void adata_processing()
{
Receive();
            // 传感器1数据处理
          if (-180<=adata&&adata<=180)
             {   oldYPR= adata;
            newadata= oldYPR;
               }
         if (181<=adata&&adata<=541)
               {oldYPR= adata-361;
                newadata = oldYPR;
                }
         if (541<=adata&&adata<=902)
               {oldYPR= adata-722;
               newadata = oldYPR;
               }   
            // 传感器2数据处理
            if (-541<=adata&&adata<-180)
            {   oldYPR= adata+361;
            newadata= oldYPR;
               }
         if (-902<=adata&&adata<=-541)
               {oldYPR= adata+722;
                newadata = oldYPR;
                }
         if (adata<=-902)
               {oldYPR= adata+1083;
               newadata = oldYPR;
               }


//   Serial.print("newadata0 =");
//   Serial.print(newadata);
//   Serial.print("newadata1=");
//   Serial.print(newadata);
//   Serial.print("newadata2=");
//   Serial.print(newadata);
//   Serial.print("newadata3 =");
//   Serial.print(newadata);
//   Serial.print("newadata4=");
//   Serial.print(newadata);
//   Serial.print("newadata5=");
//   Serial.println(newadata);

         }

int Receive()
{
    //定义一个暂存数组，大小为Mirf.payload。
    byte data;
    if(Mirf.dataReady())    //等待接收数据准备好
    {
      Mirf.getData(data);    //接收数据到data数组
      //data<左移8位与data并，重组数据。
      adata = (int)((data << 8) | data);

   if(adata != oldadata)
      {
            oldadata = adata;//本次结果作为历史结果。
            //Serial.print输出数据
         // Serial.print("A0=");
         //Serial.println(adata);
            //也可以输出双字节数据
            //Serial.write(data);
            //Serial.write(data);
          return adata;
      }
   }

}

void adatae()
{
adata_processing();


if (adata==2001)
{
      //sign = false;
            adata_processing();            
            coor_num= newadata;
            coor_num= newadata;
            coor_num= newadata;
            coor_num= newadata;
            coor_num= newadata;
            coor_num= newadata;

   Serial.print("coor_num0 =");
   Serial.print(coor_num);
   Serial.print("coor_num1=");
   Serial.print(coor_num);
   Serial.print("coor_num2=");
   Serial.print(coor_num);
   Serial.print("coor_num3 =");
   Serial.print(coor_num);
   Serial.print("coor_num4=    ");
   Serial.print(coor_num);
   Serial.print("coor_num5=");
   Serial.println(coor_num);
}




   stand_adata_x_hand = newadata-coor_num;
   stand_adata_y_hand = newadata-coor_num;
   stand_adata_z_hand = newadata-coor_num;
   stand_adata_x_arm= newadata-coor_num;
   stand_adata_y_arm= newadata-coor_num;
   stand_adata_z_arm= newadata-coor_num;




    }

希望能对大家有用祝各位生活愉快----------------------------------------------------------------------------------------------------------------------------------------------第一次发帖不喜勿喷:D

liangzhilaohu 发表于 2017-3-13 14:30

楼主是哪个学校的，加一下qq&微信：9668168 ，我这边也有类型项目需求

jzengming 发表于 2017-3-13 14:53

liangzhilaohu 发表于 2017-3-13 14:30
楼主是哪个学校的，加一下qq&微信：9668168 ，我这边也有类型项目需求

我是东北电力大学的有兴趣可以探讨。。。

xiangabc 发表于 2017-3-16 10:28

楼主开发板用的那款呀，机械臂在哪买的？

jzengming 发表于 2017-3-16 11:33

开发板上位机用的是2560，下位机用的是nano 机械臂淘宝有售。

jzengming 发表于 2017-3-16 11:34

xiangabc 发表于 2017-3-16 10:28
楼主开发板用的那款呀，机械臂在哪买的？

开发板上位机用的是2560，下位机用的是nano 机械臂淘宝有售。

xiangabc 发表于 2017-3-17 08:49

jzengming 发表于 2017-3-16 11:34
开发板上位机用的是2560，下位机用的是nano 机械臂淘宝有售。

都略贵，700-1000RMB左右

jzengming 发表于 2017-3-25 16:37

我买的才400多呀

菜菜1314 发表于 2018-11-1 10:18

您好，能不能加一下我的qq1316883352，有问题想请教一下您

查看完整版本: 6轴体感式机械臂