本文要介紹的是一種行走的四足機(jī)器人“機(jī)器貓”。3DOF 腿，IR 接收器都連接到 Pro Mini。

準(zhǔn)備工作

我想做一個(gè)四足行走的機(jī)器人，更像是“哺乳動(dòng)物”的風(fēng)格，而不是普通的“蜘蛛”或“昆蟲(chóng)”。靈感來(lái)自著名的波士頓動(dòng)力機(jī)器人和其他四足研究機(jī)器人。制作這樣的機(jī)器人非常具有挑戰(zhàn)性，因?yàn)樗苋菀滓驗(yàn)橹匦母吆湍_在身體下方而翻倒，而不是伸展到角落。

展開(kāi)來(lái)看，最終的目的還是使用 Arduino 和低成本的微型伺服系統(tǒng)制造一個(gè)廉價(jià)的機(jī)器人。這個(gè)解決方案當(dāng)然有它的局限性，不能指望它是完美的。但我今后還準(zhǔn)備再設(shè)法制造了一些機(jī)器人，用很少的預(yù)算做最好的事情本身對(duì)我來(lái)說(shuō)就是一個(gè)挑戰(zhàn)。

同時(shí)我很早就發(fā)現(xiàn)，還需要對(duì)反向運(yùn)動(dòng)學(xué) （IK） 進(jìn)行研究以使其正確。該代碼具有一組方程式，可根據(jù)所需的腳部運(yùn)動(dòng)計(jì)算關(guān)節(jié)角度。這些可以進(jìn)一步用于一些重復(fù)性任務(wù)的功能，例如進(jìn)行身體運(yùn)動(dòng)（將四只腳向相反方向移動(dòng)）和進(jìn)行完整的腳部運(yùn)動(dòng)（向上抬起以指定方向移動(dòng)并再次放下）。

下一個(gè)挑戰(zhàn)是進(jìn)行步態(tài)研究，即根據(jù)身體和足部運(yùn)動(dòng)定義機(jī)器人應(yīng)該如何行走和轉(zhuǎn)動(dòng)。我的機(jī)器人一直使用靜態(tài)穩(wěn)定的步態(tài)。當(dāng)時(shí)一只腳被抬起并放在一個(gè)新的位置。身體靠在其他三只腳上，為了不翻倒重心，必須保持在這些腳形成的三腳架內(nèi)。我開(kāi)發(fā)了四種標(biāo)準(zhǔn)步態(tài)——向前、向后、向左和向右。這反過(guò)來(lái)又利用足部和身體運(yùn)動(dòng)功能組合成一個(gè)完整的序列。

我還設(shè)計(jì)了一個(gè)同步伺服運(yùn)動(dòng)的功能。在某些情況下，幾個(gè)伺服器在設(shè)定的時(shí)間內(nèi)做出不同的沖程。這必須同步以實(shí)現(xiàn)平穩(wěn)的運(yùn)動(dòng)。

最后但同樣重要的是，我使用了完全不受保護(hù)的鋰聚合物電池。這可能是有風(fēng)險(xiǎn)的，主要的危險(xiǎn)是放電過(guò)快或過(guò)深。但只要沒(méi)有意外短路，就可以避免第一個(gè)危險(xiǎn)。普通 R/C 電池的放電率為 25 C，在這種情況下允許 12 A。UBEC 將防止其在任何情況下高于 2 A。軟件中的監(jiān)視功能可以防止第二種危險(xiǎn)。在其中一個(gè)模擬引腳上測(cè)量電壓，如果低于 7.0 V，機(jī)器人將停止工作。

我必須強(qiáng)調(diào)，電池應(yīng)該使用專用充電器充電，并應(yīng)小心處理，切勿讓充電無(wú)人看管。電池應(yīng)從機(jī)器人上拆下（使用魔術(shù)貼安裝）并在防火袋中充電，或至少與易燃材料保持安全距離，以免火勢(shì)蔓延。還要安全地存放電池。

如果您不熟悉 LiPo 電池，請(qǐng)咨詢相關(guān)行業(yè)人士及查閱相關(guān)資料并購(gòu)買(mǎi)電池以及合適的充電器，可能還有用于充電和存放的防火袋/容器。這些物品通常充滿警告標(biāo)志。

構(gòu)建機(jī)器人

首先需要花點(diǎn)時(shí)間看一下圖片，并在開(kāi)始之前弄清楚如何組裝零件。

第一步應(yīng)組裝髖關(guān)節(jié)。我使用質(zhì)量好的雙面膠帶連接零件。它們也可以用膠水粘合，但如果需要修復(fù)無(wú)法拆卸的損壞部件，一個(gè)損壞的伺服系統(tǒng)會(huì)導(dǎo)致更換整個(gè)接頭。

將伺服支架放在一個(gè)伺服的底部，與驅(qū)動(dòng)軸對(duì)齊。然后加入另一個(gè)軸垂直的伺服。下圖顯示了前后左右的髖關(guān)節(jié)。對(duì)于另外兩個(gè)角，應(yīng)制作鏡像接頭。

在繼續(xù)之前，最好確保所有 12 個(gè)舵機(jī)都居中。最好的方法是組裝 PCB 或面包板（見(jiàn)下文），連接所有伺服系統(tǒng)并加載代碼。當(dāng) Arduino 啟動(dòng)時(shí)，所有舵機(jī)將居中（命令信號(hào) 90 度）。機(jī)器人組裝完成后，將需要稍后微調(diào)中心位置。

下一步是連接稱為大腿的部分，即腿組件的“上肢”。該部件具有與通常與伺服一起交付的伺服喇叭裝配在一起的凹槽。將喇叭粘在凹槽中。確保使用膠水來(lái)連接 3D 打印材料和喇叭制成的尼龍塑料。

大腿以 60 度角與髖關(guān)節(jié)相連。當(dāng)舵機(jī)居中時(shí)，嘗試找到一個(gè)盡可能接近該角度的位置。用提供的螺絲（通常是隨伺服提供的三個(gè)中較短的一個(gè)）將喇叭固定到伺服花鍵上。下面是兩張組裝好的大腿和臀部的照片，為了清楚起見(jiàn)，伺服喇叭沒(méi)有包括在內(nèi)。

腿的下部也應(yīng)組裝。在這種情況下，伺服系統(tǒng)使用螺釘連接到腿部。隨伺服提供螺釘（通常是兩個(gè)較長(zhǎng)的“木”螺釘）。

現(xiàn)在可以將腿組裝到身體上。我稱之為“保險(xiǎn)杠”的兩個(gè)部分位于機(jī)器人的前部和后部（就像汽車(chē)上的保險(xiǎn)杠）。它們有伺服喇叭的凹槽，就像大腿部分一樣。將角粘到它們身上。然后將大腿的伺服支撐滑入身體的相應(yīng)孔中。當(dāng)這在兩側(cè)都完成時(shí)，該組件可以由保險(xiǎn)杠固定。讓腿以大約 12 度的角度向外突出（腿的腳趾向外 20 毫米）。保險(xiǎn)杠通過(guò)使用剩余的（較長(zhǎng)的）伺服螺釘固定在車(chē)身上。

最后可以連接機(jī)器人的小腿。它們應(yīng)該與大腿相反的方向傾斜，使腳尖正好位于每個(gè)腿組件的髖關(guān)節(jié)下方。

這樣機(jī)器人就組裝好了。它應(yīng)該如下圖所示。機(jī)器人的設(shè)計(jì)與最早的概念相比略有變化。車(chē)身經(jīng)過(guò)重新設(shè)計(jì)，以簡(jiǎn)化并打造更堅(jiān)固的設(shè)計(jì)。髖關(guān)節(jié)的伺服支撐和喇叭交換了位置。所以根據(jù)3D圖像組裝，避免被照片和電影剪輯混淆。

當(dāng)然，每個(gè)關(guān)節(jié)的角度不可能完全符合要求的角度，SG-90 舵機(jī)上的花鍵數(shù)量為 21，導(dǎo)致兩個(gè)位置之間的角度為 17 度。您最多可以在 10-20 度范圍內(nèi)組裝機(jī)器人，剩余的誤差必須通過(guò)更改代碼中的中性位置來(lái)調(diào)整。再次連接所有伺服系統(tǒng)并啟動(dòng) Arduino 并檢查中性位置并在需要時(shí)進(jìn)行一些機(jī)械調(diào)整（移動(dòng)一個(gè)或兩個(gè)樣條線）可能是個(gè)好主意。與伺服系統(tǒng)一起工作時(shí)，往往會(huì)意外轉(zhuǎn)動(dòng)伺服系統(tǒng)。

連接電子設(shè)備

有兩種選擇，將所有東西都放在一個(gè)面包板上，或者使用提供的 Fritzing 文件生產(chǎn) PCB。如果在將所有電源線和地線連接到伺服系統(tǒng)時(shí)不小心，您可能會(huì)遇到面包板中的電壓?jiǎn)栴}。在極端情況下，一臺(tái)伺服器可能會(huì)消耗 600 mA 的電流，而不良的連接會(huì)導(dǎo)致行為不穩(wěn)定。PCB 的電源線有非常寬的銅跡線，因此如果您正確焊接，它將正常工作。

我的設(shè)計(jì)中沒(méi)有電源開(kāi)關(guān)。只需連接電池即可開(kāi)啟和關(guān)閉機(jī)器人。如果你想添加一個(gè)，它應(yīng)該在電池連接器之后，切斷 Arduino 和 UBEC 的 7.4 V 電源。

面包板

可以將 Pro Mini、伺服連接器和大多數(shù)其他電子設(shè)備放在一個(gè)半尺寸的面包板上。我在下圖中繪制了示意圖。確保使用短跳線，尤其是用于伺服系統(tǒng)的 5 V 電源和接地連接。伺服連接器只是超長(zhǎng)的公頭，被切成三塊并壓入面包板。

圖片中沒(méi)有顯示的是電池和UBEC。可能有一些焊接來(lái)解決這個(gè)問(wèn)題，以便將連接器配件連接到電池上。從連接器兩根跳線應(yīng)連接到面包板的下部“電源軌”，以便為 Pro Mini 供電（連接到 RAW 和 GND）。還將兩個(gè)電阻從 7.4 V 電源連接到 A0 引腳。2.2k 來(lái)自正極，1k 來(lái)自地面。這會(huì)將電壓（滿電池時(shí)超過(guò) 8 V）分壓為低于 5 V 的值，該值可由模擬引腳測(cè)量。

UBEC 的輸出端有一個(gè)伺服連接器。在上面的“電源導(dǎo)軌”上加一個(gè)兩個(gè)公頭是相當(dāng)方便的。把它放在中間的某個(gè)地方，如圖所示，以確保伺服系統(tǒng)的功率分配盡可能平衡。

IR 接收器應(yīng)連接到 A1 并具有 5V 電源。接收器上的針腳足夠長(zhǎng)，可以直接插入面包板上的孔中。

下面有一個(gè)示意圖和一張關(guān)于成品面包板外觀的圖片。請(qǐng)注意，圖片顯示了具有不同引腳和連接的舊版本機(jī)器人。它仍然提供了如何連接跳線和伺服連接器的想法。

面包板通過(guò)其自粘背面連接到車(chē)身上。定位它，使伺服系統(tǒng)連接到引腳 D3、D4 和 D5（示意圖中的右上角）的角位于機(jī)器人的前/左角，并確保電路板位于身體的中心（正確的中心重力至關(guān)重要）。

PCB

PCB 是為這個(gè)機(jī)器人定制的，帶有連接到所有伺服、IR 和電壓測(cè)量的連接器。但也有從其余引腳斷開(kāi)的連接器。如果您將來(lái)想擴(kuò)展機(jī)器人，這些可用于連接其他設(shè)備。

身體上有一些小的“墊子”，可以貼合 PCB 的角落。同樣在這里，帶有 D3 到 D5 連接器的角落應(yīng)該在前面/左側(cè)。PCB上有安裝孔，但我只在機(jī)身上使用了一塊雙面膠帶來(lái)固定它。它會(huì)留在原地。

電池

電池用魔術(shù)貼固定在底部。身體上有專門(mén)用于此的平面。7.4V/500mAh LiPo 電池的外形尺寸通常約為 55x30x10 mm（上下幾毫米），非常適合這個(gè)地方。

最后，機(jī)器人可以通過(guò)將伺服線捆成漂亮的束來(lái)“修飾”，這樣它在行走時(shí)就不會(huì)被它們絆倒。它還使機(jī)器人看起來(lái)實(shí)際上是一個(gè)四足行走的生物，而不是一堆伺服線。

敲定環(huán)節(jié)

在使用機(jī)器人之前，應(yīng)微調(diào)中心位置。這是通過(guò)在代碼中編輯數(shù)組 serverdeg0 來(lái)完成的：

const float servodeg0［12］ = {90， 90， 90， 90， 90， 90， 90， 90， 90， 90， 90， 90};

這些值的順序是 alfa、beta gamma 和前/左、后/左、前/右、后/右。所以右前方的 beta 是數(shù)組中的第八個(gè)位置或者是serveddeg0［7］（數(shù)組的編號(hào)從 0 開(kāi)始）。

還有一個(gè)稱為servodir 的數(shù)組定義了舵機(jī)的旋轉(zhuǎn)方向。

const int servodir［12］ = { +1， +1， -1， -1， -1， +1， -1， -1， -1， +1， +1， +1}; // Turning direction （positive is servo counter-clockwise）

我使用的舵機(jī)逆時(shí)針?lè)较驈?0 度移動(dòng)到 180 度。我在某處讀到有伺服系統(tǒng)朝另一個(gè)方向前進(jìn)。在這種情況下，數(shù)組servodir 必須始終更改其符號(hào)。

啟動(dòng) Arduino 并檢查所有伺服系統(tǒng)的角度。采取措施，看看一切看起來(lái)都是直的和對(duì)稱的。距離和角度應(yīng)根據(jù)下圖。

每次測(cè)量很難在精確的毫米范圍內(nèi)，在厘米范圍內(nèi)是合理的。查看需要哪些更改并將它們添加/減去數(shù)組servodeg0中的值。在一切正確之前，這肯定需要幾次迭代。你將以一個(gè)看起來(lái)像這樣的servodeg0數(shù)組結(jié)束（我的一個(gè)機(jī)器人的一段實(shí)際代碼）。最重要的是，你最終應(yīng)該擁有一個(gè)四足支撐并筆直站立的機(jī)器人。

const float servodeg0［12］ = {80， 95， 100， 100， 110， 90， 100， 115， 100， 80， 80， 100};

不過(guò)需要記住的是，一段時(shí)間后，伺服系統(tǒng)可能需要重新校準(zhǔn)。中心位置會(huì)隨著時(shí)間而漂移。只需不時(shí)檢查所有內(nèi)容是否對(duì)齊。

如果您已將所有步驟都正確執(zhí)行，但仍有一個(gè)機(jī)器人會(huì)翻倒，請(qǐng)檢查重心。可以移動(dòng)電池來(lái)平衡這一點(diǎn)，使用魔術(shù)貼是一件好事。

注意：小心對(duì)待你的鋰聚合物電池

未來(lái)的進(jìn)一步改進(jìn)

通過(guò)在這里講述我的機(jī)器人后，我還邀請(qǐng)了人們改進(jìn)設(shè)計(jì)，或者添加更多功能，或者做一個(gè)稍微不同的布局（更大、更小、更酷）。該代碼應(yīng)該可以在布局或大小略有不同的機(jī)器人上重復(fù)使用。下面的草圖顯示了代碼中的不同常量。如果制造了具有不同措施的機(jī)器人，所有 IK 和運(yùn)動(dòng)功能應(yīng)該仍然有效。它還顯示坐標(biāo)已定義，x 指向正向。

當(dāng)然，如果你愿意為這個(gè)機(jī)器人添加更多功能那會(huì)很有趣。遙控器上有幾個(gè)按鈕可以被賦予功能。

我個(gè)人嘗試模擬輸入。我還使用了“走路時(shí)轉(zhuǎn)身”的步態(tài)，以便能夠在一定程度上引導(dǎo)機(jī)器人，或者在陀螺儀或指南針的幫助下糾正路線偏差。我還添加了一個(gè)超聲波傳感器和自主行為（避開(kāi)障礙物）。不過(guò)目前的項(xiàng)目是將模擬控制與自主相結(jié)合，并通過(guò)智能手機(jī)控制一切。

代碼部分：

/* An IR controlled version of the KITtyBot 2.

It uses Arduino Pro Mini and the PCB board designed by me (Fritzing sketch Kittybotmini.fzz)
It is based on the previous robots KITtyBot and KITtyBot mini using an IR remote to control the robot
It uses a NEC (Adafruit) remote and the IRLib2 libraries, see https://github.com/cyborg5/IRLib2.
Download IRLib2 libraries from the repository and install them according to the instructions.
The general dimensions are similar to the original KITtyBot but there is a displacment
between the gamma and alfa axis of 12 mm (the servos mounted on top of each other)
I have conitiously tweeked the gaits for walking and turning but I so far feel this has given the most stable behaviour.
Created by Staffan Ek 2017
*/

#include
#include
#include // First include the decode base
#include // Include only the protocol you are using

#define MY_PROTOCOL NEC //Defines the IR control (NEC)

long Previous;

IRrecv My_Receiver(A1);//Receive on pin A0
IRdecodeNEC My_Decoder;

const int servonum = 12; // The amount of servos

Servo servo[servonum]; // Create servo object
const float servodeg0[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
// Neutral positions for the servos adjusted from nominal 90 degrees (a calibration is needed to adjust these values)
float servodegnew[servonum]; // The desired servo position in degrees
float servodegold[servonum]; // The old (or current) servo position
// Update values below to the KITtyBot mini
const int servodir[12] = { +1, +1, -1, -1, -1, +1, -1, -1, -1, +1, +1, +1}; // Turning direction (positive is servo counter-clockwise)
const float pi = 3.1416;
const float alfa0 = pi / 6; // The neutral position of alfa (30 deg)
const float beta0 = pi / 3; // The neutral position of beta (60 deg)
const float jointlength = 50; // The length of a leg part (both have the same length)
const float width = 120; // The width (distance between feet in y direction, with toeout0 added)
const float leng = 120; // The length (disatnce between feet in x direction)
const float distag = 12; // Distance between alfa and gamma axis
const float toeout0 = 20; // The outward distance of feet from the gamma servo centre (the distance the foot is pointed outwards)
const float leglength0 = 2 * jointlength * cos(alfa0);
const float gamma0 = asin(toeout0 / (leglength0 + distag)); // The neutral position of gamma (due to toe out 20 mm and distag 12 mm)
const float bodyradius = sqrt(pow((width / 2), 2) + pow((leng / 2), 2)); // The length of diagonal (distance from centre to foot corner)
const float phi0 = atan(width / leng); // The angle of bodyradius vs the x (forward pointing) axis
const float height0 = sqrt(pow(leglength0 + distag, 2) - pow(toeout0, 2)); // The normal height of robot (if any angles or distances are changed this must be updated)
float leglength [4] = {sqrt(pow(height0, 2) + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2)),
sqrt(pow(height0, 2) + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2))
};
// Start values of leglength
unsigned long timestep = 500; // Time taken by each sequence (when using servomove())
int steplength = 40; //The length of a step in x direction during walking (forward and reverse creep)
float phi = 20; // turnangle during turning (in degrees, not radians!)
// Variable for movement
float footpos[12]; // Foot positions, order LeftFrontxyz, LeftRearxyz, RightFrontxyz, RightRearxyz
float stepturn[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Foot movement in case of a turn
// The foot positions are calibrated with their respective start positions
const float jointangle0[12] = {alfa0, beta0, 0, alfa0, beta0, 0, alfa0, beta0, 0, alfa0, beta0, 0};
float jointangle[12]; //Using a vector for angles, order LeftFrontAlfaBetaGamma etc

const int voltagepin = A0; // The assigned pin for voltage meassure
int lowvolt = 0; // A variable that stops the robot if the voltage goew <7.0 V

int mode = 0; // The current ordered walking mode; forward, reverse, left, right

void setup() {
Serial.begin(9600);
Serial.println("KITtyBot mini"); //These lines are just to check the configuration. Can be deleted.
Serial.print("Gamma0: ");
Serial.println(gamma0);
Serial.print("Leglength0: ");
Serial.println(leglength0);
Serial.print("Bodyradius: ");
Serial.println(bodyradius);
Serial.print("Phi0: ");
Serial.println(phi0);
Serial.print("Height0: ");
Serial.println(height0);

servo[0].attach(3);
servo[1].attach(4);
servo[2].attach(5);
servo[3].attach(6);
servo[4].attach(7);
servo[5].attach(8);
servo[6].attach(2);
servo[7].attach(A3);
servo[8].attach(12);
servo[9].attach(11);
servo[10].attach(10);
servo[11].attach(9);

for (int i = 0; i < servonum; i++) { // Centre all values and the output to the serovs
servodegnew[i] = servodeg0[i];
servodegold[i] = servodegnew[i];
servo[i].write(servodegnew[i]);
}
delay(5000);
My_Receiver.enableIRIn(); // Start the receiver

}

void loop() {
voltmeasure(); // Check voltage at least here
while (lowvolt == 0) { // Proceed only if there is enough power
bodyxyz(0, 0, 0); // Just make sure everything is centered
servomove();
mode = 0;
IRread();
Serial.print("Mode: "); // Only for monitoring in the serial console, can be deleted
Serial.println(mode);
switch (mode) {
case 1: forwardcreep(); break;
case 2: reversecreep(); break;
case 3: leftturn(); break;
case 4: rightturn(); break;
}
voltmeasure();
delay(500);
}

if (lowvolt == 1) { // Got to "rest". A clear signal that battery needs charging
bodyxyz (0, 0, -30); // Lower body, a clear signal that it has gone to rest
servomove();
}
}

// Below are the functions called in correct order in order to calculate new angles
void lengthangles() {
// Front left foot
jointangle[2] = gammaleft(footpos[1], footpos[2]);
leglength[0] = legleft(footpos[0], footpos[2], jointangle[2]);
jointangle[1] = beta(leglength[0]);
jointangle[0] = alfafront(footpos[0], jointangle[1], leglength[0]);
// Rear left foot
jointangle[5] = gammaleft(footpos[4], footpos[5]);
leglength[1] = legleft(footpos[3], footpos[5], jointangle[5]);
jointangle[4] = beta(leglength[1]);
jointangle[3] = alfarear(footpos[3], jointangle[4], leglength[1]);
// Front rigth foot
jointangle[8] = gammaright(footpos[7], footpos[8]);
leglength[2] = legright(footpos[6], footpos[8], jointangle[8]);
jointangle[7] = beta(leglength[2]);
jointangle[6] = alfafront(footpos[6], jointangle[7], leglength[2]);
// Rear right foot
jointangle[11] = gammaright(footpos[10], footpos[11]);
leglength[3] = legright(footpos[9], footpos[11], jointangle[11]);
jointangle[10] = beta(leglength[3]);
jointangle[9] = alfarear(footpos[9], jointangle[10], leglength[3]);
}

// Functions used to calculate IK

// Gamma, the hip servo "on top"
float gammaleft (float dy, float dz) {
float gresult = atan((toeout0 + dy) / (height0 - dz)) - gamma0;
return gresult;
}

float gammaright(float dy, float dz) {
float gresult = gamma0 - atan((toeout0 - dy) / (height0 - dz));
return gresult;
}

//Calculating leg length (distance alfa axis to toe)
float legleft(float dx, float dz, float gamma) {
float lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 + gamma)), 2) + pow(dx, 2));
if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher than possible some following functions become unstable
return lresult;
}

float legright(float dx, float dz, float gamma) {
float lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 - gamma)), 2) + pow(dx, 2));
if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher than possible some following functions become unstable
return lresult;
}

// Beta, the "knee joint"
float beta(float leg) {
float bresult = 2 * acos(leg / (2 * jointlength));
return bresult;
}

// Alfa, The other hip servo
float alfafront(float dx, float beta, float leg) {
float aresult = (beta / 2) - asin(dx / leg);
return aresult;
}

float alfarear(float dx, float beta, float leg) {
float aresult = (beta / 2) + asin(dx / leg);
return aresult;
}

// Giving foot positions based on a turning angle f (in degrees). Stepturn is the used to make footpos values
void turnpos(float f) {
stepturn[0] = bodyradius * cos(phi0 + (f * pi / 180)) - leng / 2;
stepturn[1] = bodyradius * sin(phi0 + (f * pi / 180)) - width / 2;
stepturn[3] = bodyradius * cos(pi - phi0 + (f * pi / 180)) + leng / 2;
stepturn[4] = bodyradius * sin(pi - phi0 + (f * pi / 180)) - width / 2;
stepturn[6] = bodyradius * cos(2 * pi - phi0 + (f * pi / 180)) - leng / 2;
stepturn[7] = bodyradius * sin(2 * pi - phi0 + (f * pi / 180)) + width / 2;
stepturn[9] = bodyradius * cos(pi + phi0 + (f * pi / 180)) + leng / 2;
stepturn[10] = bodyradius * sin(pi + phi0 + (f * pi / 180)) + width / 2;
}

// Calculates servo positions (in degrees) based on joint angles in the fucntion above
void servopos() {
for (int i = 0; i < 12; i++) {
servodegnew[i] = servodeg0[i] + servodir[i] * (jointangle[i] - jointangle0[i]) * 180 / pi;
}
}

// The servo algorithm for controlled and syncronized movements. All servos should reach their end position at the end of a timestep
void servomove() {
int servotimeold[servonum]; // Local variable for time of last servo position
int servotimenew[servonum]; // Local variable for the current time when the servo i positioned
int SERVOPULSE[servonum]; // Local variable to write to the servo driver
float servodeg[servonum]; // Local variable for the current servo position
float servodegspeed[servonum]; // Local variable for the desired servo speed degress per millisecond
unsigned long starttime = millis(); // Time stamp the start of the algorithm
unsigned long timenow = starttime; // Resetting time now
for (int i = 0; i < servonum; i++) {
servodegspeed[i] = (servodegnew[i] - servodegold[i]) / timestep; // Calculate the desired servo speed
servodeg[i] = servodegold[i]; // Resetting the servo position
servotimeold[i] = starttime; // Resetting the time
}
while ((timenow - starttime) < timestep) { // Loop continues until the time step is fulfilled
for (int i = 0; i < servonum; i++) { // Iterate through each servo
servotimenew[i] = millis(); // Get a time stamp
servodeg[i] += servodegspeed[i] * (servotimenew[i] - servotimeold[i]);
// Calculate a new position based on the desired speed and elapsed time
servo[i].write(servodeg[i]); // Position servo
servotimeold[i] = servotimenew[i]; // Resetting the old servo time for the next iteration
}
timenow = millis();
// Get a time stamp after all servos has been iterated to use in the while case.
}
for (int i = 0; i < servonum; i++) { // Make on last iteration to assure that the servos reached their end positions
servo[i].write(servodegnew[i]); // Position servo
servodegold[i] = servodegnew[i]; // Resetting the current position for future iterations
}
}

// A servomove without timing, use when no synchronous moving is needed, i.e. lifting/moving one leg
void servomovefast() {
for (int i = 0; i < servonum; i++) { // Make on last iteration to assure that the servos reached their end positions
servo[i].write(servodegnew[i]); // Position servo
servodegold[i] = servodegnew[i]; // Resetting the current position for future iterations
}
delay(100); // Just give a reasonable time for servos to reach endpoint before moving on.
}

// Calculates a foot position (xyz coordiantes)
void footxyz(int i, float x, float y, float z) {
footpos[3 * i] = x;
footpos[3 * i + 1] = y;
footpos[3 * i + 2] = z;
lengthangles();
servopos();
}

// Calculates foot movement, adding desired value to current position
void footmovexyz(int i, float x, float y, float z) {
footpos[3 * i] += x;
footpos[3 * i + 1] += y;
footpos[3 * i + 2] += z;
lengthangles();
servopos();
}

// Calculates body positioning according to xyz coordinates.
void bodyxyz(float x, float y, float z ) {
//Note: Moving body means moving the feet in the other direction, hence minus signs in all foot positions
for (int i = 0; i < 4; i++) {
footpos[3 * i] = -x;
footpos[3 * i + 1] = -y;
footpos[3 * i + 2] = -z;
}
lengthangles();
servopos();
}

// Calculates body movement, adding cooridinate to existing position.
void bodymovexyz(float x, float y, float z ) {
//Note: Body move mean moving the feet in the other direction, hence minus signs in all foot positions
for (int i = 0; i < 4; i++) {
footpos[3 * i] -= x;
footpos[3 * i + 1] -= y;
footpos[3 * i + 2] -= z;
}
lengthangles();
servopos();
}

// Calculates a twist on the body the desired angle phi
void bodytwist(float f) {
// Again here the movement is in minus driection from the foot positions
turnpos(-f);
for (int i = 0; i < 12; i++) {
footpos[i] += stepturn[i];
}
lengthangles();
servopos();
}

// Does a footmovement; lifts move xy and puts down foot
void footstep (int i, float x, float y) {
footmovexyz(i, 0, 0, 30);
servomovefast();
footmovexyz(i, x, y, 0);
servomovefast();
footmovexyz(i, 0, 0, -30);
servomovefast();
}

// Does a footmovement based on the disired turning angle, moves the foot along the turning arc
void (footstepangle(int i, float f)) {
turnpos(f);
footmovexyz(i, 0, 0, 30);
servomovefast();
footmovexyz(i, stepturn[3 * i], stepturn [3 * i + 1], 0);
servomovefast();
footmovexyz(i, 0, 0, -30);
servomovefast();
}

// Checks voltage, in case of low battery lowvolt variable changes
void voltmeasure() {
/* Note: The 7.6 V battery is conneced via a 2.2k resistance from BAT to voltagepin and 1.0k to GND
This gives the 5 V analog input a 16 volt measure range*/
float voltsig = analogRead(voltagepin);
float voltage = voltsig * 16 / 1023.0;
Serial.print("Battery: ");
Serial.println(voltage);
if (voltage < 7.0) {
lowvolt = 1;
}
else {
lowvolt = 0;
}
}

// The IR read function, based on Adafruits example sketch
void IRread() {
if (My_Receiver.getResults()) {
Serial.print("Recieving ");
Serial.println (My_Decoder.value);
if (My_Decoder.decode()) {
if (My_Decoder.value == 0xFFFFFFFF) { // Detects if the button is still pressed and keeps the value
My_Decoder.value = Previous;
}
switch (My_Decoder.value) { //Detects if an arrow button is pressed and sets mode parameter
case 0xfda05f: mode = 1; break;
case 0xfdb04f: mode = 2; break;
case 0xfd10ef: mode = 3; break;
case 0xfd50af: mode = 4; break;
}
Previous = My_Decoder.value;
}
My_Receiver.enableIRIn();
}
else {
mode = 0;
}
}

// A gait for forward creeping
void forwardcreep() {
bodymovexyz(steplength / 4, -toeout0, 0); // Starts to position for forward walking, leaning to the right
servomove();
footstep(1, steplength / 2, 0); // Moving rear left leg one half step length
footstep(0, steplength / 2, 0); // And the front left
bodymovexyz(steplength / 4, 2 * toeout0, 0); // Shifting body forward and to the left (in order to move the right feet later)
servomove();
while (mode == 1) {
// Here the while loop starts, repeaetd as long as fwd is ordered (mode 1)
footstep(3, steplength, 0); // Moving rear right forward
footstep(2, steplength, 0); // Moving front right forward
bodymovexyz(steplength / 2, -2 * toeout0, 0); // Shifting body forward and to the right
servomove();
footstep(1, steplength, 0); // Moving rear left forward
footstep(0, steplength, 0); // Moving front left forward
bodymovexyz(steplength / 2, 2 * toeout0, 0); // Shifting body forward and to the left
servomove();
// The robot has the same position as before the while loop but has moved on steplength forward.
IRread(); // If there is still a forward command (mode ==1) the sequence should be repeated
}
// The while loop ends and it assumes normal postion
/* bodymovexyz(0, 10, 0);*/
footstep(3, steplength / 2, 0); // Taking half steps to make all legs neutral
footstep(2, steplength / 2, 0);
bodyxyz(0, 0, 0); // Centering body
servomove();
// Leaving gait mode
}

// A similar gait for reverse walking (not commented as much look at forwardcreep
void reversecreep() {
bodymovexyz(-steplength / 4, -toeout0, 0); // Starts to position for forward walking
servomove();
footstep(0, -steplength / 2, 0);
footstep(1, -steplength / 2, 0);
bodymovexyz(-steplength / 4, 2 * toeout0, 0);
servomove();
while (mode == 2) {
// Here the while loop starts, repeaetd as long as reverse is ordered (mode 2)
footstep(2, -steplength, 0);
footstep(3, -steplength, 0);
bodymovexyz(-steplength / 2, -2 * toeout0, 0);
servomove();
footstep(0, -steplength, 0);
footstep(1, -steplength, 0);
bodymovexyz(-steplength / 2, 2 * toeout0, 0);
servomove();
IRread(); // If mode == 2 the while loop continues
}
// The while loop ends and it assumes normal postion
/* bodymovexyz(0, 10, 0);*/
footstep(2, -steplength / 2, 0);
footstep(3, -steplength / 2, 0);
bodyxyz(0, 0, 0);
servomove();
// Leaving gait mode
}

// Doing a turn to the left the desired phi angle
void leftturn() {
while (mode == 3) {
// While loop as long as the left button is pressed
bodyxyz(toeout0 / 2, toeout0, 0); // Lean left before doing anything
servomove();
footstepangle(3, phi); // Move rear right foot into new position
footstepangle(2, phi); // Move front right foot into new position
footxyz(0, -toeout0 / 2 - stepturn[0], toeout0 - stepturn[1], 0);
footxyz(1, -toeout0 / 2 - stepturn[3], toeout0 - stepturn[4], 0);
footxyz(2, -toeout0 / 2, toeout0, 0);
footxyz(3, -toeout0 / 2, toeout0, 0);
// Twisting body and lean left. Written in absolute coordinates to minmize errors.
servomove(); // Do the actual servo command
footstepangle(0, phi); // Move front left foot
footstepangle(1, phi); // Move rear left foot
IRread(); // Check is left button is still pressed (mode == 3), repeat while loop
}
bodyxyz(0, 0, 0); // Centre body when turning is finished
servomove();
}

//Doing a right turn. Should be identical to left turn but with different commands. Testing both at the moment.
void rightturn() {
while (mode == 4) {
// While loop as long as the right button is pressed
bodyxyz(-toeout0 / 2, toeout0, 0); // Lean left before doing anything
servomove();
footstepangle(2, -phi); //Move front right foot
footstepangle(3, -phi); //Move rear right foot
footxyz(0, toeout0 / 2 - stepturn[0], toeout0 - stepturn[1], 0);
footxyz(1, toeout0 / 2 - stepturn[3], toeout0 - stepturn[4], 0);
footxyz(2, toeout0 / 2, toeout0, 0);
footxyz(3, toeout0 / 2, toeout0, 0);
// Twisting body and lean left. Written in absolute coordinates to minmize errors.
servomove(); // Do the actual servo command
footstepangle(1, -phi); //Move rear left foot
footstepangle(0, -phi); //Move front left foot
IRread(); // Check is rightt button is still pressed (mode == 4), repeat while loop
}
bodyxyz(0, 0, 0);
servomove();
}