PAJ7620U2+蓝牙主从机实现远程手势控制

目录

​​主机：​​

​​从机： ​​

​​电机测试： ​​

---

这是个4舵机驱动的水上仿生鱼。

两块arduino通过蓝牙通信，岸上的arduino连接paj7620u2进行手势控制。

水下的arduino连接驱动执行设备（舵机和电机）对机器鱼直接控制。

主机：

主机为一块arduino连接上paj7620u2手势传感器模块。

#include <Wire.h>
#include "paj7620.h"
#include<SoftwareSerial.h>

//新建一个Serial_p对象，rx:6,tx:5
//连接蓝牙的master
SoftwareSerial Serial_p(6,5);

#define GES_REACTION_TIME   500       // You can adjust the reaction time according to the actual circumstance.
#define GES_ENTRY_TIME      800       // When you want to recognize the Forward/Backward gestures, your gestures' reaction time must less than GES_ENTRY_TIME(0.8s). 
#define GES_QUIT_TIME     1000

//上:a 下:b 左:c 右:d 前:e
char command_char='e';

void setup()
{
  uint8_t error = 0;

  Serial.begin(9600);
  Serial_p.begin(9600);
  Serial.println("Serial baud:9600");
  Serial.println("Underwater fish master program!");

  error = paj7620Init();      // initialize Paj7620 registers
  if (error) 
  {
    Serial.print("INIT ERROR,CODE:");
    Serial.println(error);
  }
  else
  {
    Serial.println("INIT OK");
  }
  Serial.println("Please input your gestures:\n");
}

void loop()
{
  uint8_t data = 0, data1 = 0, error;
  
  error = paj7620ReadReg(0x43, 1, &data);       // Read Bank_0_Reg_0x43/0x44 for gesture result.
  if (!error) 
  {
    switch (data)                   // When different gestures be detected, the variable 'data' will be set to different values by paj7620ReadReg(0x43, 1, &data).
    {
      case GES_RIGHT_FLAG:
        delay(GES_ENTRY_TIME);
        paj7620ReadReg(0x43, 1, &data);
        if(data == GES_FORWARD_FLAG) 
        {
          Serial.println("Forward");
          Serial_p.print('e');
          delay(GES_QUIT_TIME);
        }
        else if(data == GES_BACKWARD_FLAG) 
        {
          Serial.println("Backward");
          delay(GES_QUIT_TIME);
        }
        else
        {
          Serial.println("Right");
          Serial_p.print('d');
        }          
        break;
      case GES_LEFT_FLAG: 
        delay(GES_ENTRY_TIME);
        paj7620ReadReg(0x43, 1, &data);
        if(data == GES_FORWARD_FLAG) 
        {
          Serial.println("Forward");
          Serial_p.print('e');
          delay(GES_QUIT_TIME);
        }
        else if(data == GES_BACKWARD_FLAG) 
        {
          Serial.println("Backward");
          delay(GES_QUIT_TIME);
        }
        else
        {
          Serial.println("Left");
          Serial_p.print('c');
        }          
        break;
      case GES_UP_FLAG:
        delay(GES_ENTRY_TIME);
        paj7620ReadReg(0x43, 1, &data);
        if(data == GES_FORWARD_FLAG) 
        {
          Serial.println("Forward");
          Serial_p.print('e');
          delay(GES_QUIT_TIME);
        }
        else if(data == GES_BACKWARD_FLAG) 
        {
          Serial.println("Backward");
          delay(GES_QUIT_TIME);
        }
        else
        {
          Serial.println("Up");
          Serial_p.print('a');
        }          
        break;
      case GES_DOWN_FLAG:
        delay(GES_ENTRY_TIME);
        paj7620ReadReg(0x43, 1, &data);
        if(data == GES_FORWARD_FLAG) 
        {
          Serial.println("Forward");
          Serial_p.print('e');
          delay(GES_QUIT_TIME);
        }
        else if(data == GES_BACKWARD_FLAG) 
        {
          Serial.println("Backward");
          delay(GES_QUIT_TIME);
        }
        else
        {
          Serial.println("Down");
          Serial_p.print('b');
        }          
        break;
      case GES_FORWARD_FLAG:
        Serial.println("Forward");
        Serial_p.print('e');
        delay(GES_QUIT_TIME);
        break;
      case GES_BACKWARD_FLAG:     
        Serial.println("Backward");
        delay(GES_QUIT_TIME);
        break;
      case GES_CLOCKWISE_FLAG:
        Serial.println("Clockwise");
        break;
      case GES_COUNT_CLOCKWISE_FLAG:
        Serial.println("anti-clockwise");
        break;  
      default:
        paj7620ReadReg(0x44, 1, &data1);
        if (data1 == GES_WAVE_FLAG) 
        {
          Serial.println("wave");
        }
        break;
    }
  }
  delay(100);
}

从机： 

从机为一块arduino连接若干舵机。

#include <Servo.h>
#include<SoftwareSerial.h>

//定义舵机速度
#define servo_speed 10

//定义限位开关引脚
#define swit1 10

#define water 11

#define servo1_pin 6
#define servo2_pin 7
#define servo3_pin 8
#define servo4_pin 9

//舵机1的摆动幅度
#define servo1_min -30
#define servo1_max +30

//舵机2的摆动幅度
#define servo2_min -30
#define servo2_max +30

//舵机3的摆动幅度
#define servo3_min -60
#define servo3_max +60

//舵机4的摆动幅度
#define servo4_min -30
#define servo4_max +30

#define TRIGGER A0
#define ECHO A1

#define dis_min 20        //超声波避障距离
#define dis_max 180       //超声波检测最大值

//丝杆总脉冲数量
#define step_sum 204800

//上浮下潜丝杆半个行程
#define step_sx 2048

//定义电机转速，数字越小转速越高
#define speed_motor 10

#define motor_step_pin 2
#define motor_dir_pin 3

int step_now=0;


//左舵机
Servo myservo1;
//右舵机
Servo myservo2;
//尾巴舵机第一节
Servo myservo3;
//尾巴舵机第二节
Servo myservo4;

//新建一个Serial_p对象，rx:12,tx:13
//连接蓝牙的master
SoftwareSerial Serial_p(12,13);

int dis_u=30;

//蓝牙指令   a-上  b-下  c-左  d-右  e-停
char command_char='e';

char command_char_last='e';

int servo_jiaozhun[4]={0,0,0,0};

bool bizhang=0;

unsigned long time_last=0;

int dis(){
  
//  float dist=0;
//  for(int i=0;i<3;i++)
//  {
    digitalWrite(TRIGGER, LOW);
    delayMicroseconds(2);
    //将trigPin电平拉高10微秒
    digitalWrite(TRIGGER, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER, LOW);
    // 读取echoPin电平,以毫秒为单位返回声波往返时间
    // 计算距离
    Serial.println(millis());
    float distance = pulseIn(ECHO,HIGH)/58.0; //算成厘米 
    Serial.println(millis());
    Serial.print("distance=");
    Serial.println(distance);
    distance=(int(distance * 100.0))/100.0;//保留两位小数 
//    dist+=distance;
//  }
//  return int(dist/3.0);
  return int(distance);
}

void myservo_init(){
  
  myservo1.attach(servo1_pin);
  myservo2.attach(servo2_pin);
  myservo3.attach(servo3_pin);
  myservo4.attach(servo4_pin);

  myservo1.write(90+servo_jiaozhun[1]);
  myservo1.write(90+servo_jiaozhun[2]);
  myservo1.write(90+servo_jiaozhun[3]);
  myservo1.write(90+servo_jiaozhun[4]);
  delay(500);

  Serial.println("Servo init successful!");
}

//步进电机限位开关检测
bool swit1_jc(){
  if(digitalRead(swit1)==HIGH)
  {
    delay(3);
    if(digitalRead(swit1)==HIGH)
    {
      Serial.println("swit 1 on");
      return true;
    }
    else 
    return false;
  }
  return false;
}

//dir为方向，如果方向反了，就改变这个参数
void motor_step(bool dir){
  digitalWrite(motor_dir_pin,dir);
  digitalWrite(motor_step_pin,HIGH);
  delay(speed_motor);
  digitalWrite(motor_step_pin,LOW);
  delay(speed_motor);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial_p.begin(9600);
  //初始化电机引脚
  pinMode(motor_step_pin,OUTPUT);
  digitalWrite(motor_step_pin,LOW);
  pinMode(motor_dir_pin,OUTPUT);
  digitalWrite(motor_dir_pin,LOW);
  
  Serial.println("Motor pin init successful!");
  
  //初始化舵机引脚
  pinMode(servo1_pin,OUTPUT);
  digitalWrite(servo1_pin,LOW);

  pinMode(servo2_pin,OUTPUT);
  digitalWrite(servo2_pin,LOW);

  pinMode(servo1_pin,OUTPUT);
  digitalWrite(servo2_pin,LOW);

  pinMode(servo1_pin,OUTPUT);
  digitalWrite(servo2_pin,LOW);

  Serial.println("Servo pin init successful!");
  
  //初始化限位开关和水滴传感器引脚
  pinMode(swit1,INPUT);
  pinMode(water,INPUT);
  Serial.println("Swit1 pin and water pin init successful!");
  //初始化超声波传感器引脚
  pinMode(TRIGGER,OUTPUT);
  pinMode(ECHO,INPUT);
  Serial.println("TRIGGER pin and ECHO pin init successful!");

 
  //丝杆步进电机复位
//  while(!(swit1_jc()))
//  {
//    motor_step(true);
//  }
//    for(int i=0;i<step_sum/2;i++)
//      motor_step(false);
//  Serial.println("Step motor init successful!");

  myservo_init();
  
  time_last=millis();
  
  Serial.println("Robot init successful!");
  
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0)
    command_char=Serial.read();

   Serial.print("Read distance qian:");
   Serial.println(millis());
  
    if(millis()-time_last>=10000)
    {
      //读取前方障碍物距离
      dis_u=dis();
      time_last=millis();
    }
    
  Serial.print("Read distance zhong:");
  Serial.println(millis());
  //如果距离小于阈值，则当前命令改为左转
  if((dis_u>0)&&(dis_u<dis_max))
  {
    if((dis_u<dis_min)&&(bizhang==0))
    {
      command_char_last=command_char;
      command_char='c';
      bizhang=1;
    }
    if((dis_u>dis_min)&&(bizhang==1))
    {
      command_char=command_char_last;
      bizhang=0;
    }
  }

  Serial.print("Read distance hou:");
  Serial.println(millis());
    
    for(int i=0;i<30;i++)
    {
      Serial.print(millis());
      if(digitalRead(water)==0)
     {
        if(command_char=='a')
      {
        if(step_now>=-step_sx)
        {
          motor_step(true);
          step_now--;
        }
      }
     else if(command_char=='b')
     {
      if(step_now<=step_sx)
        {
          motor_step(false);;
          step_now++;
        }
     }
     else if(command_char=='c')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]);
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]-map(i,0,30,0,servo2_max));
      
      myservo1.write(90+servo_jiaozhun[1]);
      myservo2.write(90+servo_jiaozhun[2]-map(i,0,30,0,servo2_max));
     }
     else if(command_char=='d')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]+map(i,0,30,0,servo1_max));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]);
      
      myservo1.write(90+servo_jiaozhun[1]+map(i,0,30,0,servo1_max));
      myservo2.write(90+servo_jiaozhun[2]);
     }
     else if(command_char=='e')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]+map(i,0,30,0,servo1_max));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]-map(i,0,30,0,servo2_max));
      
      myservo1.write(90+servo_jiaozhun[1]+map(i,0,30,0,servo1_max));
      myservo2.write(90+servo_jiaozhun[2]-map(i,0,30,0,servo2_max));
     }
  
     
//      Serial.print("servo 3 :");
//      Serial.print(90+servo_jiaozhun[3]+map(i,0,30,0,servo3_max));
//      Serial.print("servo 4 :");
//      Serial.println(90+servo_jiaozhun[4]+map(i,0,30,0,servo3_max));
      
      myservo3.write(90+servo_jiaozhun[3]+map(i,0,30,0,servo3_max));
      myservo4.write(90+servo_jiaozhun[4]+map(i,0,30,0,servo3_max));
     }
     delay(servo_speed);
    }

    for(int i=0;i<60;i++)
    {
      Serial.print(millis());
       if(digitalRead(water)==0)
     {
        if(command_char=='a')
      {
        if(step_now>=-step_sx)
        {
          motor_step(true);
          step_now--;
        }
      }
     else if(command_char=='b')
     {
      if(step_now<=step_sx)
        {
          motor_step(false);
          step_now++;
        }
     }
     else if(command_char=='c')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]);
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]+map(i,0,60,servo2_min,servo2_max));
      
      myservo1.write(90+servo_jiaozhun[1]);
      myservo2.write(90+servo_jiaozhun[2]+map(i,0,60,servo2_min,servo2_max));
     }
     else if(command_char=='d')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]-map(i,0,60,servo1_min,servo1_max));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]);
      
      myservo1.write(90+servo_jiaozhun[1]-map(i,0,60,servo1_min,servo1_max));
      myservo2.write(90+servo_jiaozhun[2]);
     }
     else if(command_char=='e')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]-map(i,0,60,servo1_min,servo1_max));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]+map(i,0,60,servo2_min,servo2_max));
      
      myservo1.write(90+servo_jiaozhun[1]-map(i,0,60,servo1_min,servo1_max));
      myservo2.write(90+servo_jiaozhun[2]+map(i,0,60,servo2_min,servo2_max));
     }
  
    
//      Serial.print("servo 3 :");
//      Serial.print(90+servo_jiaozhun[3]-map(i,0,60,servo3_min,servo3_max));
//      Serial.print("servo 4 :");
//      Serial.println(90+servo_jiaozhun[4]-map(i,0,60,servo3_min,servo3_max));
      
      myservo3.write(90+servo_jiaozhun[3]-map(i,0,60,servo3_min,servo3_max));
      myservo4.write(90+servo_jiaozhun[4]-map(i,0,60,servo3_min,servo3_max));
     }
     delay(servo_speed);
    }

    for(int i=0;i<30;i++)
    {
      Serial.print(millis());
       if(digitalRead(water)==0)
     {
        if(command_char=='a')
      {
        if(step_now>=-step_sx)
        {
          motor_step(true);
          step_now--;
        }
      }
     else if(command_char=='b')
     {
        if(step_now<=step_sx)
        {
          motor_step(false);
          step_now++;
        }
     }
     else if(command_char=='c')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]);
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]-map(i,0,30,servo2_min,0));
      
      myservo1.write(90+servo_jiaozhun[1]);
      myservo2.write(90+servo_jiaozhun[2]-map(i,0,30,servo2_min,0));
     }
     else if(command_char=='d')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]+map(i,0,30,servo1_min,0));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]);
      
      myservo1.write(90+servo_jiaozhun[1]+map(i,0,30,servo1_min,0));
      myservo2.write(90+servo_jiaozhun[2]);
     }
     else if(command_char=='e')
     {
//      Serial.print("servo 1 :");
//      Serial.print(90+servo_jiaozhun[1]+map(i,0,30,servo1_min,0));
//      Serial.print("servo 2 :");
//      Serial.print(90+servo_jiaozhun[2]-map(i,0,30,servo2_min,0));
      
      myservo1.write(90+servo_jiaozhun[1]+map(i,0,30,servo1_min,0));
      myservo2.write(90+servo_jiaozhun[2]-map(i,0,30,servo2_min,0));
     }
  
    
//      Serial.print("servo 3 :");
//      Serial.print(90+servo_jiaozhun[3]+map(i,0,30,servo3_min,0));
//      Serial.print("servo 4 :");
//      Serial.println(90+servo_jiaozhun[4]+map(i,0,30,servo4_min,0));
      
      myservo3.write(90+servo_jiaozhun[3]+map(i,0,30,servo3_min,0));
      myservo4.write(90+servo_jiaozhun[4]+map(i,0,30,servo4_min,0));
     }
     delay(servo_speed);
    }
}

电机测试： 

这个程序主要测试步进电机转向和步数。

#define stp 2
#define dir 3

//定义限位开关引脚
#define swit1 10

//步进电机限位开关检测
bool swit1_jc(){
  if(digitalRead(swit1)==HIGH)
  {
    delay(3);
    if(digitalRead(swit1)==HIGH)
    {
      Serial.println("swit 1 on");
      return true;
    }
    else 
    return false;
  }
  return false;
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(stp,OUTPUT);
  digitalWrite(stp,LOW);
  pinMode(dir,OUTPUT);
  digitalWrite(dir,LOW);
  pinMode(swit1,INPUT);
  while(!swit1_jc())
    {
      digitalWrite(dir,true);
      digitalWrite(stp,HIGH);
      delay(10);
      digitalWrite(stp,LOW);
      delay(10);
    }
    for(int i=0;i<=500;i++)
    {
      digitalWrite(dir,false);
      digitalWrite(stp,HIGH);
      delay(10);
      digitalWrite(stp,LOW);
      delay(10);
    }
}

void loop() {
  // put your main code here, to run repeatedly:
 
}