//Code by Wyatt felt for controlling Twisty Balloon Pneumatic Actuator
// April 19, 2012


// The first three entries of the matrices below define the pins that will be used for the "back right" or "back left" legs
// the last four entries are place holders for the instant in time when they will: begin to extend, stop extending, begin to retract, stop retracting


float BR[7] = {7,8,11,0,0,0,0};  // Pressure valve pin, exaust valve pin, pump pin, 0 , 0 , 0 ,0 
float BL[7] = {9,10,11,0,0,0,0}; // pressure valve pin, exaust valve pin, pump pin, 0 , 0 , 0 ,0 

boolean brV,blV; // the booleans indicate wheter the pressure valve corresponding to their leg is open or not (used to decide whether pump should remain on)


double runTime;  // holds the value of the largest time instant before restarting loop

double legCount = 0;  // holds the value of the current time signature
float strideCount = 0; // counts the number of full actuation cycles that the program has been through.


void setup() {
    for(int i=5; i<12; i++) {   //sets the pins to output pins
           
  pinMode(i, OUTPUT);
  }
  
   
  walkSt();  // this function defines the times you want the legs to move at
  

  
  
 
 legCount = 0;
 strideCount = 0;
  // initialize serial communication:
  Serial.begin(9600);  
}

void loop() {
 
  

if (legCount==BR[3]) extend(BR);  
if (legCount==BR[4]) hold(BR); 
if (legCount==BR[5]) retract(BR); 

if (legCount==BL[3]) extend(BL);  
if (legCount==BL[4]) hold(BL); 
if (legCount==BL[5]) retract(BL); 




if (legCount > (runTime+1))
{
  legCount = 0;
  strideCount ++;
  walkSt();
}

legCount ++;


delay(1);

}



void walkSt()
{
  
  
const float ETime = 15000;  //Extension time, how long the legs should extend for in thousanths of a second (i.e 15 seconds)
const float RTime = 9000;   // Retraction time, ""                  "" retract for  ""
const float DTime = 5000;   // Delay time, how long to wait before beginning to extend 
const float HTime = 5000;   // Hold time, how long to hold the leg in the extended position before retracting
  
  BR[3] = DTime; // when to extend leg start leg
  BR[4] = BR[3] + ETime; //when to stop extending it
  BR[5] = BR[4] + HTime; // when to retract it
  BR[6] = BR[5] + RTime; // when to stop retracting it
  
  BL[3] = BR[4]; // when to extend leg start leg (you could also define it as Dtime*2)
  BL[4] = BL[3] + ETime; //when to stop extending it
  BL[5] = BL[4] + HTime; // when to retract it
  BL[6] = BL[5] + RTime; // when to stop retracting it
  
runTime = 0;
if ((BR[5] > runTime) && (BR[6] < (BR[5] + BR[1]))) runTime = BR[5];   // I don't really remember my logic here but I'm defining when to start the loop over again :)
if ((BR[6] > runTime) && (BR[6] >= (BR[5] + BR[1]))) runTime = BR[6];

if ((BL[5] > runTime) && (BL[6] < (BL[5] + BL[1]))) runTime = BL[5];
if ((BL[6] > runTime) && (BL[6] >= (BL[5] + BL[1]))) runTime = BL[6];

}

void extend(float leg[])
{
  
            
           
                 
                 if (leg[0] == BL[0]) 
                 {
                    Serial.println("                                                                  Back Left Pressure on ");                     Serial.println(legCount);
                    Serial.println("                            Back Pump on ");                     Serial.println(legCount);
                   blV = true;
                   
                  
                 }
                                
                  if (leg[0] == BR[0])
                 {
                    Serial.println("                                        Back Right Pressure on ");                     Serial.println(legCount);
                    Serial.println("                            Back Pump on ");                     Serial.println(legCount);
                   brV = true;
                  
                 }
                 
          
         
             digitalWrite(leg[0],HIGH);  // turn pressure valve HIGH
             digitalWrite(leg[1],LOW);   // turn exhaust valve LOW 
             digitalWrite(leg[2],HIGH);  // make sure pump is on
                
                 
                 
    
  }

void hold(float leg[])
{

            
             
                                    
                 
                 if (leg[0] == BL[0]) 
                 {
                      Serial.println("                                                                  Back Left Pressure off ");                     Serial.println(legCount);
                   if (brV == false)  // turn the pump off if the other leg isn't inflating
                  {
                    
                    Serial.println("                            Back Pump off ");                     Serial.println(legCount);
                   digitalWrite(leg[2],LOW);
                  
                  }
                   blV = false;
                  
                 }
                                
                  if (leg[0] == BR[0])
                 {
                      Serial.println("                                        Back Right Pressure off ");                     Serial.println(legCount);
                   if (blV == false)  // turn the pump off if the other leg isn't inflating
                  {
                   
                    Serial.println("                            Back Pump off ");                     Serial.println(legCount);
                    digitalWrite(leg[2],LOW);
                    
                  }
                   brV = false;
                 }
                      
                    
        digitalWrite(leg[0],LOW); // turn off the pressure valve
    
  }
  
void retract(float leg[])
{
         
             
                
            
                        
                        
                 
                 if (leg[1] == BL[1]) 
                 {
                   if(digitalRead(leg[1]) == LOW)   Serial.println("                                                                  Back Left Exhaust on ");                     Serial.println(legCount);
                   if (brV == false)  // turn the pump off if the other leg isn't inflating
                  {
                    
                    Serial.println("                            Back Pump off ");                     Serial.println(legCount);
                   digitalWrite(leg[2],LOW);
                  
                  }
                   blV = false;
                  
                 }
                                
                  if (leg[1] == BR[1])
                 {
                   if(digitalRead(leg[1]) == LOW)   Serial.println("                                        Back Right Exhaust on ");                     Serial.println(legCount);
                   if (blV == false)  // turn the pump off if the other leg isn't inflating
                  {
                   
                    Serial.println("                            Back Pump off ");                     Serial.println(legCount);
                    digitalWrite(leg[2],LOW);
                   
                  }
                   brV = false;
                 }
                      
                      
             digitalWrite(leg[0],LOW); // turn off pressure valve
             digitalWrite(leg[1],HIGH);  // turn on exhaust valve
            
                
                 
    
  }