如何使用Arduino和微型伺服系統(tǒng)制造一個小貓機器人

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

準備工作

我想做一個四足行走的機器人，更像是“哺乳動物”的風格，而不是普通的“蜘蛛”或“昆蟲”。靈感來自著名的波士頓動力機器人和其他四足研究機器人。制作這樣的機器人非常具有挑戰(zhàn)性，因為它很容易因為重心高和腳在身體下方而翻倒，而不是伸展到角落。

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

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

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

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

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

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

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

構(gòu)建機器人

首先需要花點時間看一下圖片，并在開始之前弄清楚如何組裝零件。

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

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

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

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

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

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

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

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

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

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

連接電子設備

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

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

面包板

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

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

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

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

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

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

PCB

PCB 是為這個機器人定制的，帶有連接到所有伺服、IR 和電壓測量的連接器。但也有從其余引腳斷開的連接器。如果您將來想擴展機器人，這些可用于連接其他設備。

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

電池

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

最后，機器人可以通過將伺服線捆成漂亮的束來“修飾”，這樣它在行走時就不會被它們絆倒。它還使機器人看起來實際上是一個四足行走的生物，而不是一堆伺服線。

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

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

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

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

還有一個稱為servodir 的數(shù)組定義了舵機的旋轉(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）

我使用的舵機逆時針方向從 0 度移動到 180 度。我在某處讀到有伺服系統(tǒng)朝另一個方向前進。在這種情況下，數(shù)組servodir 必須始終更改其符號。

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

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

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

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

如果您已將所有步驟都正確執(zhí)行，但仍有一個機器人會翻倒，請檢查重心?？梢砸苿与姵貋砥胶膺@一點，使用魔術(shù)貼是一件好事。

注意：小心對待你的鋰聚合物電池

未來的進一步改進

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

當然，如果你愿意為這個機器人添加更多功能那會很有趣。遙控器上有幾個按鈕可以被賦予功能。

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

代碼部分：

/* 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();
}