机器人制作开源方案 | Delta型腿机器狗实现原地动作
本帖最后由 机器谱 于 2023-7-24 12:56 编辑1. 功能说明
本文示例将实现R322样机Delta型腿机器狗原地摆臂、原地圆形摆动、原地蹲起、原地踏步的功能。
https://28846868.s21i.faiusr.com/3/ABUIABADGAAg1raZpQYohP7LkgMwsAQ4owM!400x400.gif.webp原地摆臂
https://28846868.s21i.faiusr.com/3/ABUIABADGAAg97aZpQYoxv_11QEwsAQ4owM!400x400.gif.webp原地圆形摆动https://28846868.s21i.faiusr.com/3/ABUIABADGAAg7beZpQYomOTwqgMwsAQ4owM!400x400.gif.webp原地蹲起https://28846868.s21i.faiusr.com/3/ABUIABADGAAgm7iZpQYopOaUHjCwBDijAw!400x400.gif.webp原地踏步
2. 电子硬件
本实验中采用了以下硬件：
主控板Basra主控板（兼容Arduino Uno）
扩展板Bigfish2.1扩展板
SH-SR舵机扩展板
传感器近红外传感器
六轴陀螺仪
电池7.4v锂电池、11.1V动力电池
其它电压显示器
电路连接说明：Delta型腿机器狗的电路连接方式可参考上文【R322】Delta型腿机器狗-全动作展示【https://www.robotway.com/h-col-242.html】
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg9L2ZpQYorN7YxQEwvAY4qgQ!600x600.png.webp
3. 功能实现
编程环境：Arduino 1.8.0
下面提供一个Delta型腿机器狗原地动作（原地摆臂、原地圆形摆动、原地蹲起、原地踏步）的参考例程，具体实验效果可参考演示视频。
① 原地摆臂例程（parallel_dog_liftLeg.ino）：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 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 机器谱 2023-06-26 https://www.robotway.com/
------------------------------*/
/*****
Copyright 2017 Robot TIme
摆臂例程
*****/
#include "Tlc5940.h"
#include "tlc_servos.h"
#include <math.h>
#include "types.h"
#include "config.h"
// 相关函数声明
/***** 红外相关函数 *****/
void IRInit(); //红外初始化
void enableIR(); //红外使能
void disableIR(); //关闭红外
void updateIR(); //红外避障更新动作
/***** 平衡相关函数 *****/
void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
void readGyroSerial(); //读取陀螺仪串口消息
void adjustAct(); //平衡调节动作
/****** 腿部动作相关函数 *****/
void setTurnLeftFlag(bool flag); //修改左转状态标志位
void setTurnRightFlag(bool flag); //修改右转状态标志位
void leg1(); //更新1号腿（左前）位置
void leg2(); //更新2号腿（左后）位置
void leg3(); //更新3号腿（右前）位置
void leg4(); //更新4号腿（右后）位置
bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
/***** 整机动作相关函数 *****/
void dogReset(Point3d initPos, uint waitTime); //复位动作
void dogInit(); //初始化动作
void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
void liftShoulder(uint height, uint times); //原地摆臂动作
//动作周期计数器
int cycleCount;
//复位计数器
void resetCycleCount()
{
cycleCount = -1;
}
void updateCycleCount()
{
cycleCount++;
}
//当前运动状态
dogMode currentMode;
//切换运动状态
void setMode(dogMode mode)
{
if (mode == currentMode) return;
if (mode == DOG_MODE_TURN_LEFT)
{
setTurnLeftFlag(true);
setTurnRightFlag(false);
} else if (mode == DOG_MODE_TURN_RIGHT)
{
setTurnLeftFlag(false);
setTurnRightFlag(true);
} else {
setTurnLeftFlag(false);
setTurnRightFlag(false);
}
if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
{
disableIR();
} else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
{
switchAdjustStat(ADJUST_STAT_LEG);
dogReset({0, 0, Leg_Init_Z_Pos}, 200);
}
currentMode = mode;
}
void updateMode()
{
if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
}
void setup()
{
//陀螺仪连接串口，波特率115200
Serial.begin(115200);
//舵机驱动板初始化
Tlc.init(0);
tlc_initServos(); // Note: this will drop the PWM freqency down to 50Hz.
//红外传感器初始化
IRInit();
//大狗身体初始化
dogInit();
//原地摆臂动作10次后停止
liftShoulder(40, 10);
while(1);
}
void loop()
{
}② 原地圆形摆动例程（parallel_dog_drawCircle.ino）：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 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 机器谱 2023-06-26 https://www.robotway.com/
------------------------------*/
/*****
Copyright 2017 Robot TIme
原地圆形摆动例程
*****/
#include "Tlc5940.h"
#include "tlc_servos.h"
#include <math.h>
#include "types.h"
#include "config.h"
// 相关函数声明
/***** 红外相关函数 *****/
void IRInit(); //红外初始化
void enableIR(); //红外使能
void disableIR(); //关闭红外
void updateIR(); //红外避障更新动作
/***** 平衡相关函数 *****/
void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
void readGyroSerial(); //读取陀螺仪串口消息
void adjustAct(); //平衡调节动作
/****** 腿部动作相关函数 *****/
void setTurnLeftFlag(bool flag); //修改左转状态标志位
void setTurnRightFlag(bool flag); //修改右转状态标志位
void leg1(); //更新1号腿（左前）位置
void leg2(); //更新2号腿（左后）位置
void leg3(); //更新3号腿（右前）位置
void leg4(); //更新4号腿（右后）位置
bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
/***** 整机动作相关函数 *****/
void dogReset(Point3d initPos, uint waitTime); //复位动作
void dogInit(); //初始化动作
void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
void liftShoulder(uint height, uint times); //原地摆臂动作
//动作周期计数器
int cycleCount;
//复位计数器
void resetCycleCount()
{
cycleCount = -1;
}
void updateCycleCount()
{
cycleCount++;
}
//当前运动状态
dogMode currentMode;
//切换运动状态
void setMode(dogMode mode)
{
if (mode == currentMode) return;
if (mode == DOG_MODE_TURN_LEFT)
{
setTurnLeftFlag(true);
setTurnRightFlag(false);
} else if (mode == DOG_MODE_TURN_RIGHT)
{
setTurnLeftFlag(false);
setTurnRightFlag(true);
} else {
setTurnLeftFlag(false);
setTurnRightFlag(false);
}
if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
{
disableIR();
} else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
{
switchAdjustStat(ADJUST_STAT_LEG);
dogReset({0, 0, Leg_Init_Z_Pos}, 200);
}
currentMode = mode;
}
void updateMode()
{
if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
}
void setup()
{
//陀螺仪连接串口，波特率115200
Serial.begin(115200);
//舵机驱动板初始化
Tlc.init(0);
tlc_initServos(); // Note: this will drop the PWM freqency down to 50Hz.
//红外传感器初始化
IRInit();
//大狗身体初始化
dogInit();
//原地做圆形摆动10周后停止
drawCircle(0, 0, -120, 60, 10);
while(1);
}
void loop()
{
}
③ 原地蹲起例程（parallel_dog_updown.ino）：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 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 机器谱 2023-06-26 https://www.robotway.com/
------------------------------*/
/*****
Copyright 2017 Robot TIme
原地蹲起例程
*****/
#include "Tlc5940.h"
#include "tlc_servos.h"
#include <math.h>
#include "types.h"
#include "config.h"
// 相关函数声明
/***** 红外相关函数 *****/
void IRInit(); //红外初始化
void enableIR(); //红外使能
void disableIR(); //关闭红外
void updateIR(); //红外避障更新动作
/***** 平衡相关函数 *****/
void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
void readGyroSerial(); //读取陀螺仪串口消息
void adjustAct(); //平衡调节动作
/****** 腿部动作相关函数 *****/
void setTurnLeftFlag(bool flag); //修改左转状态标志位
void setTurnRightFlag(bool flag); //修改右转状态标志位
void leg1(); //更新1号腿（左前）位置
void leg2(); //更新2号腿（左后）位置
void leg3(); //更新3号腿（右前）位置
void leg4(); //更新4号腿（右后）位置
bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
/***** 整机动作相关函数 *****/
void dogReset(Point3d initPos, uint waitTime); //复位动作
void dogInit(); //初始化动作
void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
void liftShoulder(uint height, uint times); //原地摆臂动作
//动作周期计数器
int cycleCount;
//复位计数器
void resetCycleCount()
{
cycleCount = -1;
}
void updateCycleCount()
{
cycleCount++;
}
//当前运动状态
dogMode currentMode;
//切换运动状态
void setMode(dogMode mode)
{
if (mode == currentMode) return;
if (mode == DOG_MODE_TURN_LEFT)
{
setTurnLeftFlag(true);
setTurnRightFlag(false);
} else if (mode == DOG_MODE_TURN_RIGHT)
{
setTurnLeftFlag(false);
setTurnRightFlag(true);
} else {
setTurnLeftFlag(false);
setTurnRightFlag(false);
}
if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
{
disableIR();
} else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
{
switchAdjustStat(ADJUST_STAT_LEG);
dogReset({0, 0, Leg_Init_Z_Pos}, 200);
}
currentMode = mode;
}
void updateMode()
{
if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
}
void setup()
{
//陀螺仪连接串口，波特率115200
Serial.begin(115200);
//舵机驱动板初始化
Tlc.init(0);
tlc_initServos(); // Note: this will drop the PWM freqency down to 50Hz.
//红外传感器初始化
IRInit();
//大狗身体初始化
dogInit();
//原地蹲起10次后停止
upDown(0, 0, -160, -90, 10);
while(1);
}
void loop()
{
}
④ 原地踏步例程（parallel_dog_stepping.ino）：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 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 机器谱 2023-06-26 https://www.robotway.com/
------------------------------*/
/*****
Copyright 2017 Robot TIme
原地蹲起例程
*****/
#include "Tlc5940.h"
#include "tlc_servos.h"
#include <math.h>
#include "types.h"
#include "config.h"
// 相关函数声明
/***** 红外相关函数 *****/
void IRInit(); //红外初始化
void enableIR(); //红外使能
void disableIR(); //关闭红外
void updateIR(); //红外避障更新动作
/***** 平衡相关函数 *****/
void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
void readGyroSerial(); //读取陀螺仪串口消息
void adjustAct(); //平衡调节动作
/****** 腿部动作相关函数 *****/
void setTurnLeftFlag(bool flag); //修改左转状态标志位
void setTurnRightFlag(bool flag); //修改右转状态标志位
void leg1(); //更新1号腿（左前）位置
void leg2(); //更新2号腿（左后）位置
void leg3(); //更新3号腿（右前）位置
void leg4(); //更新4号腿（右后）位置
bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
/***** 整机动作相关函数 *****/
void dogReset(Point3d initPos, uint waitTime); //复位动作
void dogInit(); //初始化动作
void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
void liftShoulder(uint height, uint times); //原地摆臂动作
//动作周期计数器
int cycleCount;
//复位计数器
void resetCycleCount()
{
cycleCount = -1;
}
void updateCycleCount()
{
cycleCount++;
}
//当前运动状态
dogMode currentMode;
//切换运动状态
void setMode(dogMode mode)
{
if (mode == currentMode) return;
if (mode == DOG_MODE_TURN_LEFT)
{
setTurnLeftFlag(true);
setTurnRightFlag(false);
} else if (mode == DOG_MODE_TURN_RIGHT)
{
setTurnLeftFlag(false);
setTurnRightFlag(true);
} else {
setTurnLeftFlag(false);
setTurnRightFlag(false);
}
if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
{
disableIR();
} else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
{
switchAdjustStat(ADJUST_STAT_LEG);
dogReset({0, 0, Leg_Init_Z_Pos}, 200);
}
currentMode = mode;
}
void updateMode()
{
if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
}
void setup()
{
//陀螺仪连接串口，波特率115200
Serial.begin(115200);
//舵机驱动板初始化
Tlc.init(0);
tlc_initServos(); // Note: this will drop the PWM freqency down to 50Hz.
//红外传感器初始化
IRInit();
//大狗身体初始化
dogInit();
//原地蹲起10次后停止
upDown(0, 0, -160, -90, 10);
while(1);
}
void loop()
{
}
4. 资料下载
资料内容：原地动作-程序源代码
资料下载地址：Delta型腿机器狗-原地动作【https://www.robotway.com/h-col-242.html】
页:
[1]