Quadruped Walking Code

// Written by Daniel Rodgers-Pryor, 2013
// This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License. To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/3.0/ or send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.


#include <Servo.h>
#include <Serial.h>

/*
 * Classes
 */

class Joint {
    private:
    Servo servo;
    int PercentageToAngle(unsigned int p) {
                if (orientation == -1) {
                        p = 100 - p;
                };

        return (((int) ((p * (float) (maxAngle - minAngle)) / 100.0f)) + offset + minAngle);
    };


    public:
        int minAngle, maxAngle, offset, orientation, maxSpeed;
    int targetAngle, currentAngle;
        float actuationSpeed;
    Joint() {
                orientation = 1;
                minAngle = 10;
                maxAngle = 160;
                offset = 0;
                maxSpeed = 500;

                actuationSpeed = 1.0f;

                targetAngle = PercentageToAngle(0);
                currentAngle = PercentageToAngle(0);
    };

        Joint& ServoPin(unsigned int servoPin){ servo.attach(servoPin); return *this; }
        Joint& SetFlipped(){ orientation *= -1; return *this; }
        Joint& Orientation(int x){ orientation = x; return *this; }
        Joint& MinAngle(int x){ minAngle = x; return *this; }
        Joint& MaxAngle(int x){ maxAngle = x; return *this; }
        Joint& MaxSpeed(int x){ maxSpeed = x; return *this; }
        Joint& Offset(int x){ offset = x; return *this; }

    void Extend(float _actuationSpeed = 1) {
        targetAngle = PercentageToAngle(100);
                actuationSpeed = _actuationSpeed;
    };
        void Mid(float _actuationSpeed = 1) {
        targetAngle = PercentageToAngle(50);
                actuationSpeed = _actuationSpeed;
    };
    void Retract(float _actuationSpeed = 1) {
        targetAngle = PercentageToAngle(0);
                actuationSpeed = _actuationSpeed;
    };

    void SetExtention(unsigned int extention, float _actuationSpeed = 1) {
        targetAngle = PercentageToAngle(extention);
                actuationSpeed = _actuationSpeed;
    };

    void Update(unsigned long timedelta) {
                currentAngle += (int) min((targetAngle - currentAngle), actuationSpeed * timedelta * maxSpeed / 1000.0f);
        servo.write(currentAngle);
    };
};

class Leg {
    public:
    Joint hip, knee;

        Leg& HipPin(unsigned int servoPin){ hip.ServoPin(servoPin); return *this; }
        Leg& KneePin(unsigned int servoPin){ knee.ServoPin(servoPin); return *this; }

    void Reach() {
        knee.SetExtention(40);
                hip.Extend(0.7f);
    };
    void Curl() {
        knee.Retract();
    };
        void Pull() {
                hip.Retract(0.7f);
        }

    void SetPose(unsigned int hipExtention, unsigned int kneeExtention) {
        hip.SetExtention(hipExtention);
        knee.SetExtention(kneeExtention);
    };

    void Update(unsigned long timedelta) {
        hip.Update(timedelta);
        knee.Update(timedelta);
    };
};


class Bot {
    public:
    Leg legs[4];

        void Splay() {
                for(int l = 0; l < 4; l++) {
                        legs[l].Reach();
                }
    };
    void Stand() {
        for(int l = 0; l < 4; l++) {
                        legs[l].Curl();
                }
    };
        void SetFlipped() {
        for(int l = 0; l < 4; l++) {
                        legs[l].hip.SetFlipped();
                        legs[l].knee.SetFlipped();
                }
    };


    void Update(unsigned long timedelta) {
        for(int l = 0; l < 4; l++) {
            legs[l].Update(timedelta);
        }
    };
};

/*
 * Setup
 */

Bot bot;
unsigned long time, timedelta;
long delayActions;


int l, i;
boolean pull;

void setup() {
        Serial.begin(9600); // Allows the use of Serial.print() debug statements using a serial monitor

        // Assign pins tot he hips and knees of each leg - THESE NEED TO BE CHOSEN FOR YOUR CONFIGURATION (see commented code at the start of loop()).
        bot.legs[0].HipPin(2).KneePin(9);
        bot.legs[1].HipPin(10).KneePin(3);
        bot.legs[2].HipPin(11).KneePin(5);
        bot.legs[3].HipPin(6).KneePin(4);


        // Set which hips are flipped aroud (this defines 'forward')
        bot.legs[2].hip.SetFlipped();
        bot.legs[3].hip.SetFlipped();

        // Limit hip motion (I needed to do this to avoid hitting the Aduino board - I'm going to look for a better mounting to avoid this).
        for(int l = 0; l < 4; l++) {
                bot.legs[l].hip.minAngle = 50;
                bot.legs[l].hip.maxAngle = 130;
        }

        // Initialise
        time = millis();
        delayActions = 0;

        l=0;
        i=0;
        pull = false;
}

/*
 * Body
 */

void loop() {
        /*
        I used this code to work out which pins were assigned to which leg (I connected them randomly)

        bot.legs[0].hip.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[1].hip.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[2].hip.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[3].hip.Extend();
        bot.Update(2000);
        delay(2000);


        bot.legs[0].knee.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[1].knee.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[2].knee.Extend();
        bot.Update(2000);
        delay(2000);

        bot.legs[3].knee.Extend();
        bot.Update(2000);
        delay(2000);
        */


        // Change in time since the last loop
        timedelta = millis() - time;
        time = millis();

        // Count down until instructions are issued again
        delayActions -= timedelta;
        if (delayActions <= 0) // Skip issuing instructions if the delay timer is positive
        {
                delayActions = 0; // Reset timer

                // Issue instructions
                if (!pull) {
                        bot.legs[(l)%4].Curl(); // Curl the knee
                        pull = true; // Set the hip to pull the leg back on the next instruction loop

                    bot.legs[(l+1)%4].Reach(); // Reach witht he next leg

                        delayActions += 100; // Wait 100ms before pulling the hip back
                } else {
                        bot.legs[(l)%4].Pull(); // Pull the hip back
                        pull = false;

                        l++; // Move to the next leg
                        delayActions += 400; // Wait 400ms before curling and reaching again.
                }
        }

        bot.Update(timedelta); // Calculate desired positions and write to the IO pins

        delay(10);
}