#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#define   NORMAL     1
#define   REVERSE    -1

// servo polarities
#define PolarityR    REVERSE
#define PolarityL    REVERSE
#define PolarityC    NORMAL

// positions for servos
#define middle       128
#define up           254
#define down         0

#define forwardR     10
#define forwardL     245

#define backwardR    245
#define backwardL    10

//servo channels
#define left         1
#define right        2
#define center       3

#define Delay        400000

// this will reverse the position of a servo if B = -1
#define DirCorrect(A,B) (((A-128)*B)+128)

void Setservo(int servo, int position);

// globally known when its here
FILE *output;

int main(void) {

    u_int8_t  angle;
    if ((output = fopen("/dev/ttyS0", "wb")) == NULL) { 
        printf("Unable to open serial port\n");
    }
    while(1){
            // lift up middle legs
            Setservo(center, DirCorrect(middle, PolarityC));  // set position
            fflush(output);       // output position
            usleep(Delay);       // wait for servo to move
            
            // push both legs forward
            Setservo(left, DirCorrect(forwardL, PolarityL));
            Setservo(right, DirCorrect(forwardR, PolarityR));
            fflush(output);
            usleep(Delay);        
    
            // tilt middle leg to one side
            Setservo(center, DirCorrect(up, PolarityC));
            fflush(output);       
            usleep(Delay);       
            
            // stroke left side
            Setservo(left, DirCorrect(backwardL, PolarityL));  
            fflush(output);      
            usleep(Delay);            
       
            // move middle leg to other side
            Setservo(center, DirCorrect(down, PolarityL));
            fflush(output);
            usleep(Delay);            
            
            // stroke right side
            Setservo(right, DirCorrect(backwardR, PolarityR));  
            fflush(output);      
            usleep(Delay);
                        
    }
    fclose(output);
    return 0;
}

// this wraps the 3 bytes that need to be sent to set a position
void Setservo(int servo, int position) {
        fputc(255, output);
        fputc(servo, output);
        fputc(position, output);
}