/* swr.io
Controller for Soft Wheel Robot
Created by: Nick Nieminen (npn9@cornell.edu)
Last updated: 05/15/2015 */

// Initializing digital pins for active-high valves controlling each leg
int pins[] = {6, 7, 8, 9, 10, 11, 12, 13};
int* rib = pins;
// rib[0] <--> pin 6   (driving)
// rib[1] <--> pin 7
// rib[2] <--> pin 8
// rib[3] <--> pin 9
// rib[4] <--> pin 10
// rib[5] <--> pin 11  (right turning channel)
// rib[6] <--> pin 12  (turning plug port)
// rib[7] <--> pin 13  (left turning channel)
int n = sizeof(pins)/sizeof(int);

// Setting up digital pins
void setup() {
  	for (int i = 0; i < n; i++) {
      	pinMode(pins[i], OUTPUT); 
  	}
}

// ******* TURNING *******

int plug = 6;
int right = 5;
int left = 7;
//low = plugged
//high = thru
int t_turnInflate = 500;

void inflate(int side) {
     digitalWrite(rib[plug], HIGH);
     digitalWrite(rib[side], HIGH);
     delay(t_turnInflate);
     digitalWrite(rib[plug], LOW);
}

void deflate(int side) {
    digitalWrite(rib[side], LOW);
}


// ******* DRIVING *******

int t_inflate=250;
int t_deflate=500;

void forward() {
    for(int d=0; d <= 4; d++){
        digitalWrite(rib[d], HIGH);
        delay(t_inflate);
        digitalWrite(rib[d], LOW);
        delay(t_deflate);
    }
}

void backward() {
    for(int b=4; b >= 0; b--){
        digitalWrite(rib[b], HIGH);
        delay(t_inflate);
        digitalWrite(rib[b], LOW);
        delay(t_deflate);
}

void loop() {

// Sequence of motion commands to drive forward, backward, forward again, turn to the right, and then turn to the left.
    for(int i=1; i <= 3; i++){
      forward();
    }

    for(int p=1; p <= 3; p++){
      backward();
    }

    for(int y=1; y <= 5; y++){
      forward();
    }

    inflate(right);
    for(int i=1; i <= 5; i++){
      forward();
    }
    deflate(right);

    inflate(left);
    for(int i=1; i <= 5; i++){
      forward();
    }
    deflate(left);
}