/*
    Ziggy 1.0 Control Program
    (c) 2001 Nick Donaldson psyclops@psyclops.com
    Created 4/15/2001
    Updated 5/15/2001
    
    Version 0.8
*/     

// define objects

// servo array
oServo servo[12] = new oServo;

// move set sub parameters
oByte setNum = New oByte;
oByte posVal = New oByte;

// misc loop/math variables
oByte i = New oByte;
oByte j = New oByte;


Sub void main(void) {

    // set up the servo object properties
    Initialize;

    // calibrate servos - uncomment after servo hookup to set centers
    //Calibrate;
    
    // stand up
    Stand;
    
    // test sub - uncomment to run test sub forever
    //Test;

    //OOPic.Delay = 500;

    //Sit;

while (1) {
        // walk forward
    for(i=0; i<=3; i++) {
        WalkForward;
    }
    
    // turn left in a circle
    for(i=0; i<=2; i++) {
        CircleLeft;
    }
    
    // walk backward
    for(i=0; i<=2; i++) {
        WalkBackward;
    }
    
    // turn right in a circle
    for(i=0; i<=2; i++) {
        CircleRight;
    }

    // walk forward
    for(i=0; i<=1; i++) {
        WalkForward;
    }
    
    // Rear Up
    RearUp;
}
    //Stand;
    
    Park;

    Off;

}


Sub void Initialize (void) {
        
    // set the fundamental timing delay
    Final tDelay = 8;

    // set leg positions
    //Final moveForward = 37;
    //Final moveBackward = 24;
    Final moveForward = 41;
    Final moveBackward = 21;
    Final halfForward = 36;
    Final halfBackward = 26;
    Final fullForward = 50;
    Final fullBackward = 14;
    Final moveCentre = 31;
    Final moveUp = 16;
    Final moveDown = 58;
    
    // set servo name/number constants
    Final sFrLeHo = 1;
    Final sFrRiHo = 2;
    Final sMiRiHo = 3;
    Final sMiLeHo = 4;
    Final sBaLeHo = 5;
    Final sBaRiHo = 6;
    Final sFrLeVe = 7;
    Final sFrRiVe = 8;
    Final sMiRiVe = 9;
    Final sMiLeVe = 10;
    Final sBaLeVe = 11;
    Final sBaRiVe = 12;
        
    // set servo input lines
    for (i.Value=1; i.Value<=12; i.Value++) {
        // servos 7-12 are on lines 17-22
        if (i.Value == 9) {
            servo[i.Value].Ioline = 23;
        }
        else if (i.Value >= 7) {
            servo[i.Value].Ioline = i.Value + 10;
        }
        else {
            servo[i.Value].Ioline = i.Value;
        }
    }
        
    // individual servo centering tweaks
    servo[sFrLeHo].Center = 34;         // front, left, horizontal
    servo[sFrRiHo].Center = 26;         // front, right, horizontal
    servo[sFrRiHo].InvertOut = cvTrue;  // front, right, horizontal; invert
    
    servo[sMiLeHo].Center = 32;         // 26 middle, left, horizontal
    servo[sMiRiHo].Center = 20;         // middle, right, horizontal
    servo[sMiRiHo].InvertOut = cvTrue;  // middle, right, horizontal; invert
    
    servo[sBaLeHo].Center = 26;         // rear, left, horizontal
    servo[sBaRiHo].Center = 23;         // rear, right, horizontal
    servo[sBaRiHo].InvertOut = cvTrue;  // rear, right, horizontal; invert
    
    
    servo[sFrLeVe].Center = 21;         // front, left, vertical
    servo[sFrRiVe].Center = 18;         // front, right, vertical
    
    servo[sMiLeVe].Center = 21;         // middle, left, vertical
    servo[sMiRiVe].Center = 19;         // middle, right, vertical
    
    servo[sBaLeVe].Center = 27;         // rear, left, vertical
    servo[sBaRiVe].Center = 15;         // rear, right, vertical
    
    // turn on servo and center it
    for (i.Value=1; i.Value<=12; i.Value++) {
        servo[i.Value] = 31;
        servo[i.Value].Operate =cvOn;
    }
}


Sub void Centre (void) {
    posVal = moveCentre;
    setNum = 1;
    MoveSet;
    setNum = 2;
    MoveSet;
    setNum = 3;
    MoveSet;
    setNum = 4;
    MoveSet;
}

Sub void Forward (void) {
    posVal = moveForward;
    setNum = 1;
    MoveSet;
    setNum = 2;
    MoveSet;
}

Sub void Backward (void) {
    posVal = moveBackward;
    setNum = 1;
    MoveSet;
    setNum = 2;
    MoveSet;
}

Sub void Up (void) {
    posVal = moveUp;
    setNum = 3;
    MoveSet;
    setNum = 4;
    MoveSet;
}


Sub void Down (void) {
    posVal = moveDown;
    setNum = 3;
    MoveSet;
    setNum = 4;
    MoveSet;
}


Sub void Stand (void) {
    // centre all servos
    for (i=1; i<=12; i++) {
        servo[i].Value = moveCentre;         
    }
    OOPic.Delay = 50;

    for (i=moveCentre; i<=moveDown; i++) {
        servo[sFrLeVe].Value = i;         
        servo[sMiLeVe].Value = i;         
        servo[sBaLeVe].Value = i;         
        OOPic.Delay = 10;
        
        i++;
        i++;
        i++;
        
        servo[sFrRiVe].Value = i;         
        servo[sMiRiVe].Value = i;         
        servo[sBaRiVe].Value = i;         
        OOPic.Delay = 10;
        i++;
        i++;
    }
}


Sub void WalkForward (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = moveForward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveBackward;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveBackward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveBackward;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = moveForward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = moveForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
}


Sub void WalkBackward (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveBackward;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = moveForward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = moveForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = moveForward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveBackward;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveBackward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
}


Sub void WalkForwardLeft (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = halfForward;
    servo[sMiRiHo] = moveForward;
    servo[sBaLeHo] = halfForward;
    
    servo[sFrRiHo] = moveBackward;
    servo[sMiLeHo] = moveCentre;
    servo[sBaRiHo] = moveBackward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveCentre;
    servo[sMiRiHo] = moveBackward;
    servo[sBaLeHo] = moveCentre;
    
    servo[sFrRiHo] = moveForward;
    servo[sMiLeHo] = halfForward;
    servo[sBaRiHo] = moveForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
}


Sub void WalkForwardRight (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = halfForward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveCentre;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveCentre;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveCentre;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = halfForward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = halfForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
}


Sub void WalkBackwardLeft (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveCentre;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = halfForward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = halfForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = halfForward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveCentre;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveCentre;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
}


Sub void CircleLeft (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveForward;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = moveBackward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = moveBackward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = moveBackward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveForward;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
}


Sub void CircleRight (void) {
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveForward;
    servo[sMiRiHo] = moveBackward;
    servo[sBaLeHo] = moveForward;
    
    servo[sFrRiHo] = moveForward;
    servo[sMiLeHo] = moveBackward;
    servo[sBaRiHo] = moveForward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 2 * tDelay;
    
    servo[sFrLeHo] = moveBackward;
    servo[sMiRiHo] = moveForward;
    servo[sBaLeHo] = moveBackward;
    
    servo[sFrRiHo] = moveBackward;
    servo[sMiLeHo] = moveForward;
    servo[sBaRiHo] = moveBackward;
    
    OOPic.Delay = 4 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 5 * tDelay;
}


Sub void RearUp (void) {
    
    // set horizontal leg positions
    servo[sFrLeHo].Value = fullForward;      
    servo[sFrRiHo].Value = fullForward;      
    servo[sMiLeHo].Value = fullForward;      
    servo[sMiRiHo].Value = fullForward;      
    servo[sBaLeHo].Value = fullBackward;      
    servo[sBaRiHo].Value = fullBackward;      
       
    // move first set of legs up then down
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaLeVe] = moveUp;
    
    OOPic.Delay = 3 * tDelay;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaLeVe] = moveDown;
    
    OOPic.Delay = 3 * tDelay;
    
    // move second set of legs up then down
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 3 * tDelay;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 3 * tDelay;
    
    // now lift back legs to rear up
    servo[sBaLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 50;
     
    // wiggle front legs
    
    for (i=0; i<=8; i++) {
        servo[sFrLeHo].Value = fullForward;
        servo[sFrRiHo].Value = moveCentre;
        OOPic.Delay = 12;
        servo[sFrLeVe].Value = moveDown;         
        servo[sFrRiVe].Value = moveUp;         
        OOPic.Delay = 12;
        servo[sFrLeHo].Value = moveCentre;
        servo[sFrRiHo].Value = fullForward;
        OOPic.Delay = 12;
        servo[sFrLeVe].Value = moveUp;
        servo[sFrRiVe].Value = moveDown;         
        OOPic.Delay = 12;
    }           

    Stand;    
}


// move sets of servos
// sets are defined as:
//
//   1 - horizontal set a
//   2 - horizontal set b
//   3 - vertical set a
//   4 - vertical set b
//
Sub void MoveSet(void) {
    
    // first, rectify set number
    // (set 3 is actually set 7 because the first servo in the vertical servos is 7)
    if (setNum >= 3) {
        servo[setNum+4] = posVal;
        servo[setNum+6] = posVal;
        servo[setNum+8] = posVal;
    }
    else {
        servo[setNum] = posVal;
        servo[setNum+2] = posVal;
        servo[setNum+4] = posVal;

    }

}


Sub void Sit(void) {
    // lay down gracefully
    posVal = moveCentre;
    setNum = 1;
    MoveSet;
    setNum = 2;
    MoveSet;

    OOPic.Delay = 50;

    for (i=moveDown; i>=moveUp; i--) {
        posVal = i;
        setNum = 3;
        MoveSet;
        OOPic.Delay = 2;
        setNum = 4;
        MoveSet;
        OOPic.Delay = 2;
    }
    
    Off;    
}


Sub void Park(void) {
    // all legs up
    servo[sFrLeHo] = moveCentre;
    servo[sFrRiHo] = moveCentre;
    servo[sMiLeHo] = moveCentre;
    servo[sMiRiHo] = moveCentre;
    servo[sBaRiHo] = moveCentre;
    servo[sBaRiHo] = moveCentre;

    servo[sFrLeVe] = moveDown;
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    servo[sBaRiVe] = moveDown;

    OOPic.Delay = 20;
    
    servo[sFrLeVe] = moveUp;
    servo[sMiRiVe] = moveUp;
    servo[sBaRiVe] = moveUp;
    
    OOPic.Delay = 10;
    
    servo[sFrLeVe] = moveDown;
    servo[sMiRiVe] = moveDown;
    servo[sBaRiVe] = moveDown;
    
    OOPic.Delay = 20;
    
    servo[sFrRiVe] = moveUp;
    servo[sMiLeVe] = moveUp;
    servo[sBaRiVe] = moveUp;

    OOPic.Delay = 20;
    
    servo[sFrRiVe] = moveDown;
    servo[sMiLeVe] = moveDown;
    servo[sBaRiVe] = moveDown;

    OOPic.Delay = 50;
    
    Off;
}


Sub void Off(void) {
    // turn all servos off
    
    for (i.Value=1; i.Value<=12; i.Value++) {
        servo[i.Value].Operate = cvOff;
    }

    // turn oopic off
    OOPic.Operate = cvOff;
}


Sub void Calibrate (void) {
    
    //servo[sMiRiVe] = moveUp;
    //OOPic.Delay = 200;
    
    // centre all legs
    Centre;
    OOPic.Delay = 1000;

    Off;
}


Sub void Test (void) {
    while (1) {
        // walk backwards to the right
        for(i=0; i<=4; i++) {
            WalkForwardRight;
        }
        // walk backwards to the left
        for(i=0; i<=4; i++) {
            WalkBackwardLeft;
        }
    }
    
    while (1) {
        // move all legs up
        Up;
        OOPic.Delay = 200;
    
        // move all legs down
        Down;
        OOPic.Delay = 200;
    }
    
    // centre all legs
    //Centre;
    //OOPic.Delay = 100;
    
    // move all legs up
    //Up;
    //OOPic.Delay = 200;

    // move all legs forward
    //Forward;
    //OOPic.Delay = 200;
    
    // move all legs back
    //Backward;
    //OOPic.Delay = 200;

    // move all legs down
    //Down;
    //OOPic.Delay = 200;

    //servo[sFrLeHo].Value = fullBackward;      
    //servo[sFrRiHo].Value = fullBackward;      

    // walk backwards to the right
    for(i=0; i<=4; i++) {
        WalkForwardRight;
    }
    
    // walk forwards to the left
    for(i=0; i<=4; i++) {
        WalkForwardLeft;
    }
    
    // walk forwards to the right
    for(i=0; i<=4; i++) {
        WalkForwardRight;
    }
    
    //OOPic.Delay = 500;

    Off;
}