Delta型腿機器狗全動作展示

1. 功能說明

本文示例將實現R322樣機Delta型腿機器狗維持身體平衡、原地圓形擺動、原地踏步、蹲起、站立、前進、后退、轉向、橫向移動、斜向移動等功能。

2. 電子硬件

本實驗中采用了以下硬件：

電路連接說明：為了便于識別控制Delta型腿機器狗，我們先對機器狗的腿位置編號（如下圖所示）：

① 硬件連接：

② 電壓顯示器與大電池連接：

③ 舵機接線位置：上面3個舵機分別連接在Bigfish擴展板的D4、D3、D8端口。

Delta型腿機器狗每條腿有4個舵機，4條腿上總共有16個舵機，將這16個舵機分別連接在SH-SR舵機擴展板的舵機接口上。

1號腿 ：s1連接口9 s2連接口8 s3連接口5 s4連接口6

2號腿 ：s1連接口18 s2連接口19 s3連接口20 s4連接口21

3號腿 ：s1連接口0 s2連接口2 s3連接口1 s4連接口3

4號腿 ：s1連接口27 s2連接口25 s3連接口26 s4連接口24

3. 功能實現

編程環境：Arduino 1.8.19

下面提供一個Delta型腿機器狗全動作展示（維持身體平衡、原地圓形擺動、原地踏步、蹲起、站立、前進、后退、轉向、橫向移動、斜向移動）的參考例程（parallel_dog_display.ino），程序源代碼及樣機3D文件資料詳見 https://www.robotway.com/h-col-242.html，具體實驗效果可參考網站演示視頻。

/*------------------------------------------------------------------------------------

  版權說明：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-07 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();


  //原地擺臂動作一次

  liftShoulder(40, 1);

  delay(500);

  //原地做圓形擺動3周

  drawCircle(0, 0, -120, 60, 3);

  delay(500);

  //原地蹲起3次

  upDown(0, 0, -160, -90, 3);

  delay(500);

  //原地踏步6次

  stepping(0, 0, -150, -100, 6);

  delay(500);



  resetCycleCount();

  enableIR();

  switchAdjustStat(ADJUST_STAT_TRACK);

  setMode(DOG_MODE_FRONT);

}


void loop()

{

  //姿態調節

  adjustAct();


  if (currentMode == DOG_MODE_STOP) return; //靜止模式不進行后續動作


  updateMode(); //切換運動模式

  //計算4條腿運動位置

  leg1();

  leg4();

  leg2();

  leg3();

  //更新所有舵機位置

  Tlc.update();

  //檢測紅外傳感器信息

  updateIR();


}


//串口與陀螺儀通信

void serialEvent() {

  readGyroSerial();

}


審核編輯 黃宇