如何让6自由度双足机器人翻跟头？
本帖最后由 机器谱 于 2023-1-5 09:28 编辑1. 功能描述控制腿部舵机协调运动，使样机做出翻跟头的动作，本功能使用6自由度双足机器人样机。
2. 电子硬件在这个示例中，采用了以下硬件，请大家参考：
主控板Basra（兼容Arduino Uno）
扩展板Bigfish2.1
电池7.4V锂电池
3. 运动控制上位机：Controller 1.0
下位机编程环境：Arduino 1.8.19
（1）初始位置的设定：使用Controller上位机对机器人进行调试，调试目标为使机器人保持直立状态，并且保持右脚的支撑臂在前。
①将Controller下位机程序servo_bigfish.ino直接下载到主控板。这段代码供Controller上位机与主控板通信，并允许调试舵机。代码如下：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2022-10-20 https://www.robotway.com/
------------------------------
实验功能： 6自由度双足行走
-----------------------------------------------------
实验接线（从行走时朝前的方向看）：
左侧髋：D4；右侧髋：D3；
左侧膝：D7；右侧膝：D5；
左侧踝：D11；右侧踝：D6
------------------------------------------------------------------------------------*/
/*
* Bigfish扩展板舵机口; 4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19
* 使用软件调节舵机时请拖拽对应序号的控制块
*/
#include <Servo.h>
#define ANGLE_VALUE_MIN 0
#define ANGLE_VALUE_MAX 180
#define PWM_VALUE_MIN 500
#define PWM_VALUE_MAX 2500
#define SERVO_NUM 12
Servo myServo;
int data_array = {0,0}; //servo_pin: data_array, servo_value: data_array;
int servo_port = {4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19};
int servo_value = {};
String data = "";
boolean dataComplete = false;
void setup() {
Serial.begin(9600);
}
void loop() {
while(Serial.available())
{
int B_flag, P_flag, T_flag;
data = Serial.readStringUntil('\n');
data.trim();
for(int i=0;i<data.length();i++)
{
//Serial.println(data);
switch(data)
{
case '#':
B_flag = i;
break;
case 'P':
{
String pin = "";
P_flag = i;
for(int i=B_flag+1;i<P_flag;i++)
{
pin+=data;
}
data_array = pin.toInt();
}
break;
case 'T':
{
String angle = "";
T_flag = i;
for(int i=P_flag+1;i<T_flag;i++)
{
angle += data;
}
data_array = angle.toInt();
servo_value)] = data_array;
}
break;
default: break;
}
}
/*
Serial.println(B_flag);
Serial.println(P_flag);
Serial.println(T_flag);
for(int i=0;i<2;i++)
{
Serial.println(data_array);
}
*/
dataComplete = true;
}
if(dataComplete)
{
for(int i=0;i<SERVO_NUM;i++)
{
ServoGo(i, servo_value);
/*********************************串口查看输出***********************************/
// Serial.print(servo_value);
// Serial.print(" ");
}
// Serial.println();
/*********************************++++++++++++***********************************/
dataComplete = false;
}
}
void ServoStart(int which){
if(!myServo.attached() && (servo_value != 0))myServo.attach(servo_port);
else return;
pinMode(servo_port, OUTPUT);
}
void ServoStop(int which){
myServo.detach();
digitalWrite(servo_port,LOW);
}
void ServoGo(int which , int where){
ServoStart(which);
if(where >= ANGLE_VALUE_MIN && where <= ANGLE_VALUE_MAX)
{
myServo.write(where);
}
else if(where >= PWM_VALUE_MIN && where <= PWM_VALUE_MAX)
{
myServo.writeMicroseconds(where);
}
}
int pin2index(int _pin){
int index;
switch(_pin)
{
case 4: index = 0; break;
case 7: index = 1; break;
case 11: index = 2; break;
case 3: index = 3; break;
case 8: index = 4; break;
case 12: index = 5; break;
case 14: index = 6; break;
case 15: index = 7; break;
case 16: index = 8; break;
case 17: index = 9; break;
case 18: index = 10; break;
case 19: index = 11; break;
}
return index;
}
下载完成后，保持主控板和电脑的USB连接，以便利用上位机进行调试。
②双击打开Controller 1.0b.exe：
③界面左上角选择：设置-面板设置，弹出需要显示的调试块，可通过勾选隐藏不需要调试的舵机块：联机-选择主控板对应端口号以及波特率。
④拖动进度条，可以观察相应的舵机角度转动，直到样机姿态达到我们的预想目标。然后勾选左下角添加-转化，获得舵机调试的数组：
⑤该数组可直接复制到后面的行走程序中“各个舵机的初始位置”部分进行使用。
（2）编写并下载翻跟头程序的代码（R213b_Somersaults.ino）到主控板：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2022-10-20 https://www.robotway.com/
------------------------------
实验功能： 6自由度双足翻跟头
-----------------------------------------------------
实验接线（从行走时朝前的方向看）：
左侧髋：D4；右侧髋：D3；
左侧膝：D7；右侧膝：D8；
左侧踝：D11；右侧踝：D12.
------------------------------------------------------------------------------------*/
#include <Servo.h>
Servo myServo;
int num = 20;
int servo_port={4,7,11,3,8,12}; //定义舵机；
int servo_num = sizeof(servo_port)/sizeof(servo_port); //servo_pin length
float value_init={86,101,88,82,92,81}; //各个舵机的初始位置
#define servo_speed 110 //舵机时间；
void setup() {
Serial.begin(9600);
reset(); //初始化舵机
delay(3000);
}
void loop() {
for(int i=0;i<3;i++) //让机器人执行三次动作
{
delay(1000);
ketou(); //机器人头着地
delay(30);
zuofanzhuan(); //机器人4、7、11舵机所对应的腿翻转
delay(30);
youfanzhuan(); //机器人3、8、12舵机所对应的腿翻转
delay(30);
qili(); //机器人起立
delay(300);
}
servo_move(86,101,88,82,92,81); //机器人翻三个跟头后一直保持直立状态
delay(1/0); //程序一直执行延时；
}
void qili() //机器人起立
{
servo_move(170,22,118,169,168,48);
servo_move(86,101,88,82,92,81);
}
void youfanzhuan() //机器人3、8、12舵机所对应的腿翻转
{
servo_move(7,176,56,169,168,48);
servo_move(170,22,118,169,168,48);
}
void zuofanzhuan() //机器人4、7、11舵机所对应的腿翻转
{
servo_move(7,176,56,5,8,113);
servo_move(7,176,56,169,168,48);
}
void ketou() //机器人头着地
{
servo_move(86,101,88,82,92,81);
servo_move(7,176,56,5,8,113);
}
void reset() //初始化舵机
{
for(int i=0; i<servo_num; i++)
{
ServoGo(i, value_init);
}
}
void ServoGo(int which , int where)//给舵机写入相应角度
{
if(where!=200)
{
if(where==201) ServoStop(which);
else
{
ServoStart(which);
myServo.write(where);
}
}
}
void ServoStart(int which)
{
if(!myServo.attached())myServo.attach(servo_port);
pinMode(servo_port, OUTPUT);
}
void ServoStop(int which) //释放舵机
{
myServo.detach();
digitalWrite(servo_port,LOW);
}
void servo_move(float value0, float value1, float value2, float value3, float value4, float value5)
{
float value_arguments[] = {value0, value1, value2, value3, value4, value5};//定义新数组
float value_delta;
for(int i=0;i<servo_num;i++)
{
value_delta = (value_arguments - value_init) / num;//给要转动的舵机写入（转动的角度/num）度，并将该度数定义为新的数组
}
for(int i=0;i<num;i++)
{
for(int k=0;k<servo_num;k++)
{
value_init = value_delta == 0 ? value_arguments : value_init + value_delta;//将新写入的舵机角度与初始化舵机角度比较，如果
//与初始化角度不相同，则初始化角度+给要转动的舵机写入（转动的角度/num）度
//组成新的数组并作为初始舵机角度数组，否则，为初始舵机角度数组。
}
for(int j=0;j<servo_num;j++)
{
ServoGo(j,value_init);
}
delay(servo_speed);
}
}
4. 资料下载资料内容：6自由度双足-翻跟头-例程源代码
资料下载链接：https://www.robotway.com/h-col-160.html
页:
[1]