目录
这是个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:
}