12_DOF舵机狗arduino uno加PCA 9685控制代码
硬件接线
参考:https://blog.csdn.net/qq_42807924/article/details/82229997
注意:
1、舵机数量多,所以电池要用供电电流大的锂离子电池,普通电池供电电流不够,舵机会不转或者乱转;
2、最好锂离子电池给舵机供电,也就是接在PCA9685板子的供电口上,arduino再另外接电池供电,那么此时上图接线的arduino的5v和PCA9685的vcc口就不要连接,但是GND要连。这样做的好处是:舵机运行的电流变化不会影响arduino板子,并且烧录程序的时候舵机不会因为arduino连接USB就乱转。
控制代码
写了一个特别简单的代码,能实现对角的两条腿同时动,前进的足端轨迹是先水平往后,再抬腿走一个抛物线迈回到起点。
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>
//这是四足狗走直线的程序
#define pi 3.1415
#define L0 15.5 //髋关节长度
#define L1 88.5 //大腿长度
#define L2 115 //小腿长度
#define STEP 150 //定义一个行走周期的脉冲总数为100
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//右前腿舵机通道,1--髋关节舵机,2-大腿舵机,3-小腿舵机;
#define R_f_1 0
#define R_f_2 1
#define R_f_3 2
#define R_b_1 3
#define R_b_2 4
#define R_b_3 5
#define L_f_1 6
#define L_f_2 7
#define L_f_3 8
#define L_b_1 9
#define L_b_2 10
#define L_b_3 11
int ledPin1 = 3;
int ledPin2 = 4;
int ledPin3 = 5;
float x0=-25.0;
float y0=145.0;
float x1=10.0;
float y1=115.0;
float h=105.0; //h是腿抬起的最高点坐标
float Px[STEP];
float Py[STEP];
void make_path()
{
//着地相,分配P/2个脉冲
int j=0;
for(float i=x0;i<x1;i=i+(x1-x0)/(STEP/2))
{
Px[j] = i;
Py[j] = y0;
// Serial.print("j= ");
// Serial.print(j);
// Serial.print(" Px[j]=");
// Serial.print(Px[j]);
// Serial.print(" Py[j]=");
// Serial.println(Py[j]);
j++;
if(j>=STEP/2) break;
}
//腾空相,分配P/2个脉冲
j=STEP/2;
for(float i=x1;i>x0;i=i-(x1-x0)/(STEP/2) )
{
Px[j] = i;
Py[j]=4*(y0-h)/pow(x1-x0,2)*pow(i-(x0+x1)/2,2)+h;
// Serial.print("j= ");
// Serial.print(j);
// Serial.print(" Px[j]=");
// Serial.print(Px[j]);
// Serial.print(" Py[j]=");
// Serial.println(Py[j]);
j++;
if(j>=STEP) break;
}
}
void setup()
{
Serial.begin(9600);
Serial.println("12 channel Servo test!");
pwm.begin();
pwm.setPWMFreq(50); // Analog servos run at ~60 Hz updates
//模拟伺服在50赫兹更新下运行
make_path();
}
//存下500个路径点
//0度:0.5/20*4096 = 102
//45度:1/20*4096 = 1/20*4096 = 204 * 0.915 = 187
//90度:1.5/20*4096 = 1.s5/20*4096 = 306 * 0.915 = 280
//135度:2/20*4096 = 2/20*4096 = 408 * 0.915 = 373
//180度:2.5/20*4096 = 2.5/20*4096 =510
//把角度值转化为4096对应的脉冲值
int angle_to_pluse(float angle)
{
float pluse;
pluse = 187.392 * (0.5 + 2*angle/pi);
return int(pluse);
}
//已以大腿舵机转动中心为原点建立直角坐标系,x轴向后,y轴向下,把足底坐标值转化为关节角度
//angle_2是小腿关节角度,angle_1是大腿关节角度
//右边两条腿逆解
float R_x_y_to_angle_1(float x ,float y)
{
float angle_1;
float angle_2;
angle_2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
double judge_expression = 4 * pow(x*(L1 + L2*cos(angle_2)), 2)-4 * (x*x+y*y)*( pow(x, 2) - pow(L2*sin(angle_2), 2) );
angle_1 = acos( ((x*(L1 + L2*cos(angle_2))) + 0.5*sqrt(judge_expression))/(x*x+y*y) ) ;
return angle_1+pi/6;
}
float R_x_y_to_angle_2(float x ,float y)
{
float angle_1;
float angle_2;
angle_2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
return angle_2;
}
//左边两条腿逆解
float L_x_y_to_angle_1(float x ,float y)
{
float angle_1;
float angle_2;
angle_2 = acos((L1*L1 + L2*L2-x*x - y*y)/(2*L1*L2));
double judge_expression = 4*pow(x*(L2*cos(angle_2) - L1),2) - 4*(2*L1*L1 + 2*L2*L2 - x*x -y*y)*(x*x - pow(L2*sin(angle_2),2));
angle_1 = acos( (x*(L2*cos(angle_2) - L1) - 0.5*sqrt(judge_expression)) / (2*L1*L1 + 2*L2*L2-x*x - y*y ) );
return angle_1;
}
float L_x_y_to_angle_2(float x ,float y)
{
float angle_1;
float angle_2;
angle_2 = acos((L1*L1 + L2*L2-x*x - y*y)/(2*L1*L2));
return angle_2;
}
//x0和y0是脚落地瞬间的位置,作为一个周期的起始位置。x1和y2是脚抬起瞬间的位置,h是腿抬起的最高点坐标
//对角步态
void loop()
{
//创建一条路径
//对角的腿动作相同,且腾空相和着地相交替轮换
for(int i=0;i<STEP/2;i++)
{
pwm.setPWM(R_f_1, 0, angle_to_pluse(pi/2-pi/20) );
pwm.setPWM(R_f_2, 0, angle_to_pluse( R_x_y_to_angle_1(Px[i] , Py[i]) ) );//着地
pwm.setPWM(R_f_3, 0, angle_to_pluse( R_x_y_to_angle_2(Px[i] , Py[i]) ) );//着地
pwm.setPWM(R_b_1, 0, angle_to_pluse(pi/2-pi/20) );
pwm.setPWM(R_b_2, 0, angle_to_pluse( R_x_y_to_angle_1(Px[i+STEP/2] , Py[i+STEP/2]) ) );//腾空
pwm.setPWM(R_b_3, 0, angle_to_pluse( R_x_y_to_angle_2(Px[i+STEP/2] , Py[i+STEP/2]) ) );//腾空
// Serial.print("i= ");
// Serial.print(i);
// Serial.print(" Px[i+STEP/2]= ");
// Serial.print(Px[i+STEP/2]);
// Serial.print(" Py[i+STEP/2]= ");
// Serial.print(Py[i+STEP/2]);
// Serial.print(" R_x_y_to_angle_1= ");
// Serial.print(R_x_y_to_angle_1(Px[i+STEP/2] , Py[i+STEP/2]) );
// Serial.print(" R_x_y_to_angle_2= ");
// Serial.println(R_x_y_to_angle_2(Px[i+STEP/2] , Py[i+STEP/2]) );
pwm.setPWM(L_f_1, 0, angle_to_pluse(pi/2+pi/20) );
pwm.setPWM(L_f_2, 0, angle_to_pluse( L_x_y_to_angle_1(Px[i+STEP/2] , Py[i+STEP/2]) ) );//腾空
pwm.setPWM(L_f_3, 0, angle_to_pluse( L_x_y_to_angle_2(Px[i+STEP/2] , Py[i+STEP/2]) ) );//腾空
pwm.setPWM(L_b_1, 0, angle_to_pluse(pi/2+pi/20) );
pwm.setPWM(L_b_2, 0, angle_to_pluse( L_x_y_to_angle_1(Px[i] , Py[i]) ) );//着地
pwm.setPWM(L_b_3, 0, angle_to_pluse( L_x_y_to_angle_2(Px[i] , Py[i]) ) );//着地
delay(4);
}
for(int i=STEP/2;i<STEP;i++)
{
pwm.setPWM(R_f_1, 0, angle_to_pluse(pi/2-pi/20));
pwm.setPWM(R_f_2, 0, angle_to_pluse( R_x_y_to_angle_1(Px[i] , Py[i]) ) );//腾空
pwm.setPWM(R_f_3, 0, angle_to_pluse( R_x_y_to_angle_2(Px[i] , Py[i]) ) );//腾空
pwm.setPWM(R_b_1, 0, angle_to_pluse(pi/2-pi/20));
pwm.setPWM(R_b_2, 0, angle_to_pluse( R_x_y_to_angle_1(Px[i-STEP/2] , Py[i-STEP/2]) ) );//着地
pwm.setPWM(R_b_3, 0, angle_to_pluse( R_x_y_to_angle_2(Px[i-STEP/2] , Py[i-STEP/2]) ) );//着地
// Serial.print("i= ");
// Serial.print(i);
// Serial.print(" Px[i-STEP/2]= ");
// Serial.print(Px[i-STEP/2]);
// Serial.print(" Py[i-STEP/2]= ");
// Serial.print(Py[i-STEP/2]);
// Serial.print(" R_x_y_to_angle_1= ");
// Serial.print(R_x_y_to_angle_1(Px[i-STEP/2] , Py[i-STEP/2]) );
// Serial.print(" R_x_y_to_angle_2= ");
// Serial.println(R_x_y_to_angle_2(Px[i-STEP/2] , Py[i-STEP/2]) );
pwm.setPWM(L_f_1, 0, angle_to_pluse(pi/2+pi/20));
pwm.setPWM(L_f_2, 0, angle_to_pluse( L_x_y_to_angle_1(Px[i-STEP/2] , Py[i-STEP/2]) ) );//着地
pwm.setPWM(L_f_3, 0, angle_to_pluse( L_x_y_to_angle_2(Px[i-STEP/2] , Py[i-STEP/2]) ) );//着地
pwm.setPWM(L_b_1, 0, angle_to_pluse(pi/2+pi/20));
pwm.setPWM(L_b_2, 0, angle_to_pluse( L_x_y_to_angle_1(Px[i] , Py[i]) ) );//腾空
pwm.setPWM(L_b_3, 0, angle_to_pluse( L_x_y_to_angle_2(Px[i] , Py[i]) ) );//腾空
delay(4);
}
}
需要这个Adafruit_PWMServoDriver.h库,没有的话在网上找,下载存在arduino的安装路径下,具体找教程。