1. 功能說明

本文示例將實(shí)現(xiàn)R322樣機(jī)Delta型腿機(jī)器狗維持身體平衡、原地圓形擺動(dòng)、原地踏步、蹲起、站立、前進(jìn)、后退、轉(zhuǎn)向、橫向移動(dòng)、斜向移動(dòng)等功能。

2. 電子硬件

本實(shí)驗(yàn)中采用了以下硬件：

電路連接說明：為了便于識(shí)別控制Delta型腿機(jī)器狗，我們先對(duì)機(jī)器狗的腿位置編號(hào)（如下圖所示）：

① 硬件連接：

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

③ 舵機(jī)接線位置：上面3個(gè)舵機(jī)分別連接在Bigfish擴(kuò)展板的D4、D3、D8端口。

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

1號(hào)腿 ：s1連接口9 s2連接口8 s3連接口5 s4連接口6

2號(hào)腿 ：s1連接口18 s2連接口19 s3連接口20 s4連接口21

3號(hào)腿 ：s1連接口0 s2連接口2 s3連接口1 s4連接口3

4號(hào)腿 ：s1連接口27 s2連接口25 s3連接口26 s4連接口24

3. 功能實(shí)現(xiàn)

編程環(huán)境：Arduino 1.8.19

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

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

  版權(quán)說明：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 機(jī)器譜 2023-06-07 https://www.robotway.com/

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

/*****

  Copyright 2017 Robot TIme

  全動(dòng)作展示例程

*****/


#include "Tlc5940.h"

#include "tlc_servos.h"

#include 


#include "types.h"

#include "config.h"


// 相關(guān)函數(shù)聲明

/***** 紅外相關(guān)函數(shù) *****/

void IRInit(); //紅外初始化

void enableIR(); //紅外使能

void disableIR(); //關(guān)閉紅外

void updateIR(); //紅外避障更新動(dòng)作

/***** 平衡相關(guān)函數(shù) *****/

void switchAdjustStat(uint stat); //切換平衡調(diào)節(jié)模式 不調(diào)節(jié)/原地調(diào)節(jié)/行進(jìn)間調(diào)節(jié)

void readGyroSerial(); //讀取陀螺儀串口消息

void adjustAct(); //平衡調(diào)節(jié)動(dòng)作

/****** 腿部動(dòng)作相關(guān)函數(shù) *****/

void setTurnLeftFlag(bool flag); //修改左轉(zhuǎn)狀態(tài)標(biāo)志位

void setTurnRightFlag(bool flag); //修改右轉(zhuǎn)狀態(tài)標(biāo)志位

void leg1(); //更新1號(hào)腿（左前）位置

void leg2(); //更新2號(hào)腿（左后）位置

void leg3(); //更新3號(hào)腿（右前）位置

void leg4(); //更新4號(hào)腿（右后）位置

bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解計(jì)算函數(shù)

/***** 整機(jī)動(dòng)作相關(guān)函數(shù) *****/

void dogReset(Point3d initPos, uint waitTime); //復(fù)位動(dòng)作

void dogInit(); //初始化動(dòng)作

void upDown(float x, float y, float z1, float z2, uint times); //蹲起動(dòng)作

void drawCircle(float ox, float oy, float z, float r, uint times); //原地圓形擺動(dòng)動(dòng)作

void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步動(dòng)作

void liftShoulder(uint height, uint times); //原地?cái)[臂動(dòng)作


//動(dòng)作周期計(jì)數(shù)器

int cycleCount;

//復(fù)位計(jì)數(shù)器

void resetCycleCount()

{

  cycleCount = -1;

}

void updateCycleCount()

{

  cycleCount++;

}


//當(dāng)前運(yùn)動(dòng)狀態(tài)

dogMode currentMode;

//切換運(yùn)動(dòng)狀態(tài)

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) //后退時(shí)關(guān)閉紅外傳感器

  {

    disableIR();

  } else if (mode == DOG_MODE_STOP) //靜止后開始原地姿態(tài)調(diào)節(jié)

  {

    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);


  //舵機(jī)驅(qū)動(dòng)板初始化

  Tlc.init(0);

  tlc_initServos();   // Note: this will drop the PWM freqency down to 50Hz.


  //紅外傳感器初始化

  IRInit();


  //大狗身體初始化

  dogInit();


  //原地?cái)[臂動(dòng)作一次

  liftShoulder(40, 1);

  delay(500);

  //原地做圓形擺動(dòng)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()

{

  //姿態(tài)調(diào)節(jié)

  adjustAct();


  if (currentMode == DOG_MODE_STOP) return; //靜止模式不進(jìn)行后續(xù)動(dòng)作


  updateMode(); //切換運(yùn)動(dòng)模式

  //計(jì)算4條腿運(yùn)動(dòng)位置

  leg1();

  leg4();

  leg2();

  leg3();

  //更新所有舵機(jī)位置

  Tlc.update();

  //檢測(cè)紅外傳感器信息

  updateIR();


}


//串口與陀螺儀通信

void serialEvent() {

  readGyroSerial();

}


審核編輯 黃宇