Project tutorial

KITtyBot © CC BY-SA

A walking four legged robot "mammal style." 3DOF legs, IR receiver all connected to a Pro Mini. Material cost less than $100.

  • 1,744 views
  • 1 comment
  • 7 respects

Components and supplies

11113 01
SparkFun Arduino Pro Mini 328 - 5V/16MHz
×1
Servo (generic)
SG-90 or 9 g servo with the same form factor
×12
Adafruit UBEC DC/DC Step-Down (Buck) Converter - 5V @ 3A output
×1
Adafruit Mini Remote Control
Of course you can use any remote. This happened to work well together with the IRLib library
×1
Adafruit IR (Infrared) Receiver Sensor - TSOP38238
×1
Mfr 25frf52 1k sml
Resistor 1k ohm
×1
Resistor 2.2k ohm
×1
7.4 V / 500 mAh LiPo Battery
A battery of roughly this capacity intended for R/C hobby will do
×1

Necessary tools and machines

3drag
3D Printer (generic)
09507 01
Soldering iron (generic)
The breadboard design might do without soldering

Apps and online services

About this project

Introduction

I wanted to do a walking four legged robot, more in a "mammal" style than the normal "spider" or "insect." The inspiration comes from the well known Boston Dynamics robots and other four legged research robots. Making a robot like this is quite challenging since it quite easily tips over due to a high center of gravity and the feet under the body instead of spreading out to the corners.

Film clip of KITtyBot2

The aim was to build a cheap robot, using Arduino and low cost micro servos. This solution of course has it's limitations. One can not expect it to be perfect but I have manage to build a few robots now that can perform the walking behavior as described in this film. And doing the best one can on a very small budget is a challenge by itself and maybe something guys on heavily funded research projects never have to face . :)

It was early identified that a study of inverted kinematics (IK) was needed in order to get it right. The code has a set of equations to calculate joint angles based on desired foot movements. These can be further used in functions for some recurring tasks like doing a body movement (moving all fours feet in the opposite direction) and making a complete foot movement (lifting upwards moving in a specified direction and putting it down again).

The next challenge is to do gait studies, i.e. to define how the robot should walk and turn in terms of body and foot movements. My robot uses statically stable gaits all the time. One foot at the time is lifted and put into a new position. The body rests on the other three feet and in order to not tip over the center of gravity must remain within the tripod that these feet form. I developed four standard gaits - forward, reverse, left and right. This in turn utilizes the foot and body movement functions to be combined into a full sequence.

I also designed a function for synchronous servo movements. In some cases several servos are making different strokes during a set time. This must be synchronized in order to achieve smooth movements.

Last but not least i use a completely unprotected LiPo battery. This can be risky, the major hazards is to dis-charge it to fast or too deeply. The first hazard is avoided as long as it isn't accidentally short-circuited. A normal R/C battery has a discharge rate of 25 C which in this case allow for 12 A. The UBEC will prevent it from being higher than 2 A in any circumstances. The second hazard is prevented by a surveillance function in the software. The voltage is measured on one of the analog pins and if gets lower than 7.0 V the robot is put to rest.

And finally I must stress that the batteries should be charged with a purpose built charger and handled with usual care, never leave charging unattended. The battery should detached from the robot (use velcro to mount it) and charged inside a fire proof bag or at least with a safe distance from flammable materials so a fire can be contained and not spread. Also store your batteries safely.

If you are not familiar with LiPo batteries consult a local R/C hobby store and buy batteries together with a suitable charger and possibly a fire proof bag/container for charging and storage. These items are often full of warning signs. Reda them and use your own good judgement. :)

Building the robot

Print the parts according to the supplied files. Take som time to look at the pictures and figure out how to assemble the parts before starting. I am Scandinavian but this instruction is far from the level of an IKEA or LEGO instruction :)

The hip joint should be assembled first. I used double sided tape of good quality to join the parts. They could be glued also but in case there would be a need to repair a broken part they are impossible to disassemble, one broken servo leads to a replacement of the complete joint.

Put the servo support on the bottom of one servo, in line with the actuation axis. Then join another servo with it's axis perpendicular. The picture below shows the hip joint for the front-right and rear-left. For the two other corners mirrored joints should be made.

Before proceeding it is good idea to make sure that all 12 servos are centered. The best way is to assemble the PCB (or breadboard, see below), connect all servos and the load the code. When the Arduino is started up all servos will center (command signal 90 degree). There will be need to fine tune the center positions later once the robot is assembled.

The next step is to attach the part called thigh, the "upper limb" of a leg assembly. This part has recesses that fit together with the servo horns that normally is delivered together with the servo. Glue horns into the recesses. Make sure to use a glue that works for joining the 3D printed material and the nylon plastic that the horn is made of. The glue gun I used worked fine, I have had some mixed success with CA glue though (some brands work, other not).

The thigh is joined to the hip joint at a 60 degree angle. Try to find a position that comes as close as possible to this angle when the servos have been centered. Secure the horn onto the servo spline with the supplied screw (often the shorter of three that are delivered with a servo). Below are two pictures of assembled thigh and hip, servo horn not included for clarity (or never modelled out of laziness from my side) .

The lower part of the leg should also be assembled. In this case a servo is attached to the leg part using screws. There are screws supplied with the servo (often two longer "wood" screws).

Now the legs can be assembled onto the body. There are two parts I called "bumper" that are on the front and rear of the robot (like the bumpers on a car). They have recesses for servo horns just like the thigh part. Glue horns into them. Then slide the servo support of an upper leg into the corresponding hole in the body. When this is done on both sides, the assembly can be secured by the bumper. Allow the legs to point out at about 12 degrees (a 20 mm toe-out of the leg). The bumper is secured to the body by using left-over (longer) servo screws.

At last the lower legs of the robot can be attached. They should be angled in the opposite direction of the thigh, making the tip of the foot being right under the hip joint of each leg assembly.

By this the robot is assembled. It should look like on the picture below. Note that the design of the robot has changed slightly compared to the top image and the film clip. The body has been redesigned to simplify and make a more robust design. The servo support and horn for the hip joint have swapped places. So assemble according to the 3D images and avoid being confused by the photos and film clips.

Of course the angles of each joint cannot be exactly at the angles required, the number of splines on a SG-90 servo is 21, leading to an angle of 17 degree between two positions. You can at best assemble the robot within 10-20 degrees, the remaining error must be adjusted by changing the neutral position in the code, see further down in this instruction. It might be a good idea to once again connect all servos and fire up the Arduino and check the neutral positions and if needed do some mechanical adjustments (moving a joint a spline or two). One tend to accidentally turn the servos when working with them.

Connecting the electronics

There are two options, have everything on one breadboard or to produce a PCB with the supplied Fritzing file. You might run into some problems with voltage in the breadboard if you don't take care when connecting all power and ground lines to the servos. In extreme cases one servo can consume 600 mA and poor connections lead to erratic behavior. The PCB have very broad copper traces for the power lines so if you just solder properly it will work fine.

There is no power switch in my design. The robot is simply turned on and off by connecting the battery. If you want to add one it should be after the battery connector, cutting of the 7.4 V supply to both the Arduino and the UBEC.

Breadboard version

It is possible to have the Pro Mini, connectors for the servos and most of the other electronics on one half-size breadboard. I draw the schematics in the picture below. Make sure to use short jumper wires, especially for the 5 V power and ground connections to the servos. The servo connectors are simply extra long male headers that are cut in pieces of three and pressed into the breadboard.

What is not shown in the picture is the battery and the UBEC. There might be some soldering to fix this in order to attach a connector fitting to the battery. From the connector two jumper wires should be connected to the lower "power rail" of the breadboard in order to feed the Pro Mini (connected to RAW and GND). Also connect two resistors from the 7.4 V supply to the A0 pin. 2.2k goes from the positive side and 1k from the ground. This will divide the voltage, which is more than 8 V on a full battery, to a value below 5 V which can be measured by the analog pin.

The output side of the UBEC has a servo connector. It is quite convenient to add a two male header on the upper "power rail". Put it somewhere in the middle like in the picture to assure that the power distribution to the servos is as balanced as possible.

The IR receiver should be connected to A1 and have 5V supply. The pins on the receiver are long enough to be fitted into holes directly on the breadboard.

There is a schematic below and a picture on how the finished breadboard might look. Note that the picture shows an older version of the robot with different pinouts and connections. It still gives an idea on how to connect jumper wires and servo connectors.

The breadboard is attached to the body with its self-adhesive backside. Orient it so that the corner with the servos connected to the pins D3, D4 and D5 (upper right in the schematic) is on the front/left corner of the robot and make sure that the board is centered on the body (correct center of gravity is vital).

PCB version

I added a Fritzing file below. This can be used to produce a PCB, either by ordering from the service available at Fritzing or by exporting files for PCB manufacturing. I did a serie of pictures to show the assembly below. The PCB is custom made for this robot with connectors to all servos, IR and voltage measurement. But there are also connectors broken out from the remaining pins. These can be used to connect other equipment if you want to expand the robot in the future.

There are small "pads" on the body the fits to the corners of the PCB. Also here the corner with the D3 to D5 connector should be on front/left. There are mounting holes on the PCB but I only used a piece of double sided tape on the body to attach it. It will stay on place.

Battery

The battery is attached to the underside with velcro. There is flat surface dedicated for this on the body. A 7.4V/500mAh LiPo battery normally has the form factor of about 55x30x10 mm (give or take of few mm) and it fits quite well into this place.

Finally the robot can be "touched up" by strapping the servo wires into nice bundles so it isn't tripped by them while walking. It also gives the robot a nice look of actually being a four legged creature walking around and not a heap of servo wires. :)

Finalising

Before using the robots the center positions should be fine tuned. This is done by editing the array servodeg0 in the code:

const float servodeg0[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; 

The values are in order alfa, beta gamma and front/left, rear/left, front/right, rear/right. So beta on the front right is the eighth position in the array or servodeg0[7] (the numbering of the array starts with 0).

There is also an array called servodir that defines rotation direction of the servos.

const int servodir[12] = {  +1, +1, -1, -1, -1, +1, -1, -1, -1, +1, +1, +1}; // Turning direction (positive is servo counter-clockwise) 

The servos I used move from from 0 to 180 degrees in counter-clockwise direction. I have read somewhere that there are servos going in the other direction. In such case the array servodir must have their signs changed all along.

Start up the Arduino and check the angles on all servos. Make measures and look the everything looks straight and symmetric. Distances and angles should be according to the picture below.

It is difficult to be within exact millimeter on each measurement, to be within a cm is reasonable.See what changes that are needed and add/subtract them to the value in the array servodeg0. This will definitely require a few iterations before everything is correct. You will end with a servodeg0 array that looks something like this (an actual piece of code from one of my robots). And most important, you should in the end have a robot that rests on all four feet and stand straight.

 const float servodeg0[12] = {80, 95, 100, 100, 110, 90, 100, 115, 100, 80, 80, 100};

Now everything is finished. Enjoy!

Just maybe a few tips on the way.

The servos might need re-calibrating after some while. The center position can drift over time. Just check if everything is aligned from time to time.

If you have made everything correct and still have a robot that will tip over, check the center of gravity. The battery can be moved to balance this, one good thing with using velcro.

And once again. Treat your LiPo batteries with care.

Further improvements

By submitting my robot here I also invite people to refine the design, either adding more functions or to do a slightly different layout (bigger, smaller, cooler looking). The code should be possible to re-use on a robot of a slightly different layout or size. The sketch below show what the different constants in the code. All IK and movement functions should still work if a robot with different measures is made. It also show the coordinates are defined, x is pointing in the forward direction.

And of course it would be interesting if people added functions to the robot. There are several buttons on the remote that could be given functions (why not dance or do some other sequence of movements if a button is pressed).

I personally experiment with analog input. I have also worked with a "turning while walking" gait to be able to steer the robot to some extent, or to correct course deviations with help of gyro or compass. And I also added an ultrasonic sensor and autonomous behavior (avoiding obstacles). My current project is to combine analog control with autonomous and have everything controlled from a smartphone. This have forced me to learn about a lot of new things (Blynk, ESP6822, serial communication between deivices etc) and hopefully I can launch a refined version of the robot (or maybe someone with better skills beat me on that thing) :)

Custom parts and enclosures

The body of the robot. Carries the PCB and battery.
Bumper
At each end of body, like bumper on a car. You should have two of these.
Servo Support
Supports the hip joint. Print four of these
The upper limb of leg. Print four.
Lower leg
The lower limb (or foot). Print four.
Assembly
All parts put together. Not for printing.

Schematics

The PCB
This is a Fritzing file to allow PCB production. Use it to order from Fritzing themselves or export to a suitable format an manufacture elsewhere.
kittybotmini_XKZYltyQVJ.fzz

Code

The code for KITtyBot2Arduino
Upload to the Arduino Mini
/* 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 IRLib.
    Version 1.51 March 2015 Copyright 2013-2015 by Chris Young http://tech.cyborg5.com/irlib/
    (It has been superseeded by IRLib2 but this sketch still uses the old library)
    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 <IRLib.h>
#include <Servo.h>

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

long Previous;

IRrecv My_Receiver(A1);//Receive on pin A0
IRdecode 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(&My_Decoder)) {
    My_Decoder.decode();
    if (My_Decoder.decode_type == MY_PROTOCOL) {
      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.resume();
  }
  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();
}

Comments

Similar projects you might like

Measure and Analyze Tide Levels with ThingSpeak and MATLAB

Project tutorial by Team MATLAB IoT

  • 3,674 views
  • 0 comments
  • 19 respects

Drive with PID Control on an Arduino Mega 2560

Project tutorial by Team MATLAB Makers

  • 3,113 views
  • 0 comments
  • 6 respects

Helping the Disabled and Chronic Patients battle the heat

Project tutorial by Manan Rai

  • 618 views
  • 0 comments
  • 6 respects

CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Project in progress by Pigeon-Kicker

  • 252 views
  • 0 comments
  • 2 respects

AutoHome - Internet of Things (IoT) for Home Automation

Project showcase by Team AutoHome

  • 350 views
  • 0 comments
  • 0 respects

Bluetooth MIDI-Operated Antique Reed Organ

Project in progress by Willem Hillier

  • 2,584 views
  • 1 comment
  • 11 respects
Add projectSign up / Login