机器人制作开源方案 | Delta型腿机器狗实现原地动作

1. 功能说明

      本文示例将实现R322样机Delta型腿机器狗原地摆臂、原地圆形摆动、原地蹲起、原地踏步的功能。

原地摆臂

原地圆形摆动

原地蹲起

原地踏步

 2. 电子硬件

本实验中采用了以下硬件：

主控板	Basra主控板（兼容Arduino Uno）
扩展板	Bigfish2.1扩展板
	SH-SR舵机扩展板
传感器	近红外传感器
	六轴陀螺仪
电池	7.4v锂电池、11.1V动力电池
其它	电压显示器

电路连接说明：Delta型腿机器狗的电路连接方式可参考上文【R322】Delta型腿机器狗-全动作展示

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 


#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 


#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 

#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 

#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()

{

}

原地动作-程序源代码资料详见 Delta型腿机器狗-原地动作