6轴体感式机械臂

jzengming

学校的项目本来想着自己搞着玩来着，结果搞着搞着没动力了，今天发到这里来。大家共同监督共同进步吧。
首先是硬件部分由于硬件部分比较简单我就不多说了直接上图了


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


//GY953模块通信子程序定义部分
#include <Wire.h>
int YPR[3];
unsigned char Re_buf[11],counter=0;
unsigned char sign=0;
int YPR2[3];
unsigned char Re_buf2[11],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[0]==0x5A&&Re_buf[1]==0x5A )        //检查帧头，帧尾
     {                 
            YPR[0]=(Re_buf[8]<<8|Re_buf[9])/100;   //合成数据，去掉小数点后2位
            sendadata( YPR[0]);
            YPR[1]=(Re_buf[6]<<8|Re_buf[7])/100;
             sendadata( YPR[1]+361);
            YPR[2]=(Re_buf[4]<<8|Re_buf[5])/100;
             sendadata( YPR[2]+722);
           Serial.print("YPR:\t");
           Serial.print(YPR[0], DEC); Serial.print("\t"); //显示航向
           Serial.print(YPR[1], DEC); Serial.print("\t"); //显示俯仰角
           Serial.print(YPR[2], DEC);                    //显示横滚角      

           delay(10);           
   }
  }


  if(sign2)
  {  // Serial.println("Y");
     sign2=0;
     if(Re_buf2[0]==0x5A&&Re_buf2[1]==0x5A )        //检查帧头，帧尾
     {                 
            YPR2[0]=(Re_buf2[8]<<8|Re_buf2[9])/100;   //合成数据，去掉小数点后2位
            sendadata( YPR2[0]-361);
            YPR2[1]=(Re_buf2[6]<<8|Re_buf2[7])/100;
             sendadata( YPR2[1]-722);
            YPR2[2]=(Re_buf2[4]<<8|Re_buf2[5])/100;
             sendadata( YPR2[2]-1083);
           Serial.print("YP:\t");
           Serial.print(YPR2[0], DEC); Serial.print("\t"); //显示航向
           Serial.print(YPR2[1], DEC); Serial.print("\t"); //显示俯仰角
           Serial.println(YPR2[2], 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[2];
    byte data[Mirf.payload];

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

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

void serialEvent3() {
  while (Serial3.available()) {   
    // Serial.println("YPR");
    Re_buf[counter]=(unsigned char)Serial3.read();
    if(counter==0&&Re_buf[0]!=0x5A) return;      // 检查帧头         
    counter++;      
    if(counter==11)                //接收到数据
    {   
       counter=0;                 //重新赋值，准备下一帧数据的接收
       sign=1;
    }      
  }
}
void serialEvent2() {
  while (Serial2.available()) {   
    // Serial.println("YPR");
    Re_buf2[counter2]=(unsigned char)Serial2.read();
    if(counter2==0&&Re_buf2[0]!=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[6];
int oldYPR[6];
int newadata[6];
int coor_num[6];
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;
int  stand_adata_x_hand , stand_adata_y_hand , stand_adata_z_hand ,
     stand_adata_x_arm  , stand_adata_y_arm ,  stand_adata_z_arm ;
int  old_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[0]= adata;
              newadata[0]= oldYPR[0];  
               }
           if (181<=adata&&adata<=541)  
               {  oldYPR[1]= adata-361;
                newadata[1] = oldYPR[1];
                }
           if (541<=adata&&adata<=902)  
               {  oldYPR[2]= adata-722;
                 newadata[2] = oldYPR[2];
               }   
              // 传感器2数据处理
            if (-541<=adata&&adata<-180)
            {   oldYPR[3]= adata+361;
              newadata[3]= oldYPR[3];  
               }
           if (-902<=adata&&adata<=-541)  
               {  oldYPR[4]= adata+722;
                newadata[4] = oldYPR[4];
                }
           if (adata<=-902)  
               {  oldYPR[5]= adata+1083;
                 newadata[5] = oldYPR[5];
               }


//     Serial.print("  newadata0 =  ");
//     Serial.print(newadata[0]);
//     Serial.print("  newadata1=  ");
//     Serial.print(newadata[1]);
//     Serial.print("  newadata2=  ");
//     Serial.print(newadata[2]);
//     Serial.print("  newadata3 =  ");
//     Serial.print(newadata[3]);
//     Serial.print("  newadata4=  ");
//     Serial.print(newadata[4]);
//     Serial.print("  newadata5=  ");
//     Serial.println(newadata[5]);

         }

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

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

}  

void adatae()
{
  adata_processing();


if (adata==2001)
{
        //  sign = false;
            adata_processing();            
            coor_num[0]= newadata[0];
            coor_num[1]= newadata[1];
            coor_num[2]= newadata[2];
            coor_num[3]= newadata[3];
            coor_num[4]= newadata[4];
            coor_num[5]= newadata[5];

     Serial.print("  coor_num0 =  ");
     Serial.print(coor_num[0]);
     Serial.print("  coor_num1=  ");
     Serial.print(coor_num[1]);
     Serial.print("  coor_num2=  ");
     Serial.print(coor_num[2]);
     Serial.print("  coor_num3 =  ");
     Serial.print(coor_num[3]);
     Serial.print("  coor_num4=    ");
     Serial.print(coor_num[4]);
     Serial.print("  coor_num5=  ");
     Serial.println(coor_num[5]);
  }




   stand_adata_x_hand = newadata[0]-coor_num[0];
   stand_adata_y_hand = newadata[1]-coor_num[1];
   stand_adata_z_hand = newadata[2]-coor_num[2];
   stand_adata_x_arm  = newadata[3]-coor_num[3];
   stand_adata_y_arm  = newadata[4]-coor_num[4];
   stand_adata_z_arm  = newadata[5]-coor_num[5];




    }

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

liangzhilaohu

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

jzengming

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

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

xiangabc

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

jzengming

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

jzengming

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

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

xiangabc

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

都略贵，700-1000RMB左右

jzengming

我买的才400多呀

菜菜1314

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