//###########################################################################//
//                                                                           //
// walk2.c - Written for SDCC compiler                                       //
//                                                                           //
// This program was written for the 8051 family of microcontrollers.  The    //
// test microcontroller was an Atmel AT89C2051.  The challenge was           //
// accurately generating simultaneous and varied pulse widths out of three   //
// output pins to control three servos with a microcontroller that did not   //
// have any PCA (Programmer Counter Array) or internal native Pulse Width    //
// modulation capabilities. The true purpose of the program is to control    //
// the Insectronics hexapod robot which uses three servos for walking. Info  //
// on the robot can be found at http://www.thinkbotics.com.  I am not        //
// affiliated with the author.                                               //
//                                                                           //
// The high level theory behind the Pulse Width functionality is that        //
// Timer 0 is set to overflow every .1 ms.  This is all the granularity      //
// needed to control the servos for this project.  By using counter          //
// variables for each servo, the number of timer overflows can be tracked    //
// and the output pins set high or low at the appropriate times to generate  //
// the correct output pulses for servo control.                              //
//                                                                           //
// This code has been tested on the above microcontroller connected to the   //
// robot itself.  The servos behave as expected and the robot walks and      //
// turns as desired.                                                         //
//                                                                           //
//###########################################################################//

#include <at89x51.h>

#define ON 1
#define OFF 0
#define HIGH 1
#define LOW 0

// Initializing variables

static char servo1_target;
static char servo1_count;
static char servo1_pulse;

static char servo2_target;
static char servo2_count;
static char servo2_pulse;

static char servo3_target;
static char servo3_count;
static char servo3_pulse;

static unsigned int speed;

// Function Prototypes

void Timer0_ISR (void) interrupt 1;
void ServoPulse (char,char,char);
void DelayMS(unsigned int);
void Walk(unsigned char, unsigned char);


// Function called when Timer 0 overflows
// The basic functionality is that the timer overflows every .1 ms.  The servos
// require pulses in a range from 2.4 ms to 1.0 ms for full rotation left and
// right (your servos may vary).  So if a 1.4 ms pulse is desired, the port is
// set high (i.e. P1.2) and left high until the timer overflows 14 times.  Then
// it is set low for the difference between 18 ms and the target.  18 ms is just
// about equivalent to a 55 Hz freqency which the servos like.  So in the case
// of a desired 1.4 ms pulse width, we turn on P1.2 for 14 timer overflows and
// then turn it off for 18 - 1.4 = 16.6 ms or 166 timer overflows. This yields
// the desired 1.4 ms pulse width at 55 Hz.

void Timer0_ISR (void) interrupt 1 {

    if(servo1_count == servo1_target) {

        if(P1_2) {  // If P1.2 is on, turn it off and start a long cycle
            P1_2 = OFF;
            servo1_target = 180 - servo1_pulse;
        }
        else {  // If P1.2 is off, turn it on a start a short cycle
            P1_2 = ON;
            servo1_target = servo1_pulse;
        }

        servo1_count = 0;
    }

    if(servo2_count == servo2_target) {

        if(P1_3) {
            P1_3 = OFF;
            servo2_target = 180 - servo2_pulse;
        }
        else {
            P1_3 = ON;
            servo2_target = servo2_pulse;
        }

        servo2_count = 0;
    }

    if(servo3_count == servo3_target) {

        if(P1_4) {
            P1_4 = OFF;
            servo3_target = 180 - servo3_pulse;
        }
        else {
            P1_4 = ON;
            servo3_target = servo3_pulse;
        }

        servo3_count = 0;
    }

    // Incrementing the servo counters

    servo1_count++;
    servo2_count++;
    servo3_count++;

    TF0 = OFF;  // Resetting the Timer 0 interrupt flag

    return;
}


// This is a wrapper function that allows easy positioning of three servos by
// the passed in arguments.  It then waits for the global setting of "speed"
// until it returns to give the servos time to get into the desired position.

void ServoPulse(char left, char right, char middle) {

    servo1_pulse = left;
    servo2_pulse = right;
    servo3_pulse = middle;

    DelayMS(speed);

    return;
}


// This is a general delay function swiped off of the web.  

void DelayMS(unsigned int ms) {
        ms;
        _asm
        mov     r0, dpl
00001$: mov     r1, #230
00002$: nop
        nop
        nop
        nop
        nop
        nop
        djnz    r1, 00002$
        djnz    r0, 00001$
        ret
        _endasm;
}

// This is yet another wrapper function to make walking calls easier.
// Essentially it is fed a number corresponding to direction and a
// duration corresponding to how many of the various walk cycles desired.
// The sequence of ServoPuls calls under each correspond to the coordinated
// servo moves to complete one cycle of the desired direction.  The direction
// numbers correspond to the numbers on a 10 key keypad.  

void Walk(unsigned char dir, unsigned char tim) {

    unsigned char x;

    for(x = 0; x < tim; x++) {

        switch(dir) { 

            case 8: // Walk Forward

                ServoPulse(12,11,17);
                ServoPulse(17,17,17);
                ServoPulse(17,17,13);
                ServoPulse(12,11,13);
                break;

            case 2: // Walk Backward

                ServoPulse(12,11,13);
                ServoPulse(17,17,13);
                ServoPulse(17,17,17);
                ServoPulse(12,11,17);
                break;

            case 6: // Turn Right

                ServoPulse(12,17,17);
                ServoPulse(17,11,17);
                ServoPulse(17,11,13);
                ServoPulse(12,17,13);
                break;

            case 4: // Turn Left

                ServoPulse(12,17,13);
                ServoPulse(17,11,13);
                ServoPulse(17,11,17);
                ServoPulse(12,17,17);
                break;

            default:
                break;
        }

    }

    return;

}

void main (void) {

    EA = ON;        // Turning on all interrupts      
    ET0 = ON;       // Turning on Timer 0 interrupt 

    TR0 = OFF;      // Shutting off Timer 0
    TF0 = OFF;      // Resetting Timer 0 interrupt flag

    TMOD &= 0xF0;   // Setting Timer 0 to 8-bit auto reload mode
    TMOD |= 0x02;

    TH0 = 0xA3;     // Setting Timer 0 initial and auto reload values
    TL0 = 0xA3;     // On an 11.0592 MHz crystal this will be .1 ms between interrupts

    P1_2 = OFF;     // Setting P1.2 low
    P1_3 = OFF;     // Setting P1.3 low
    P1_4 = OFF;     // Setting P1.4 low


    // Setting all values for servo pulse counts to 0

    servo1_pulse = 0;
    servo1_target = servo1_pulse;
    servo1_count = 0;

    servo2_pulse = 0;
    servo2_target = servo2_pulse;
    servo2_count = 0;

    servo3_pulse = 0;
    servo3_target = servo3_pulse;
    servo3_count = 0;

    speed = 8000;     // Setting the initial delay value between servo pulse changes 

    TR0 = ON;         // Turning on Timer 0


    ServoPulse(14,14,14);  // Setting all Servos to neutral position (pulse width of 1.4 ms)

    while (1) {

        Walk(8,5);    // Walk forward 5 cycles
        Walk(2,5);    // Walk backward 5 cycles
        Walk(6,3);    // Turn right 3 cycles
        Walk(4,3);    // Turn left 3 cycles
    }
}