Tri-BotHack/MotorTest

/*
  BallBot Exercise
  This is test code to exercise the WowWee Tri-Bot capabilities
  Have to push down the head to get the robot to move 
  This example code is in the public domain.
 */

// constants won't change. They're used here to 
// set pin numbers:
const int buttonPin = 3;     // the number of the pushbutton pin
const int ledPin =  13;      // the number of the LED pin

// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status
int headPush=0;  //if 0 means execute any instructions and wait, if 1 then do and check if headpushed regularly

void setup() {
  // initialize the digital pin as an output.
  pinMode(12, OUTPUT); //yellow wire to motor enable 1
  pinMode(11, OUTPUT); //teal wire to motor enable 2
  pinMode(10, OUTPUT); //blue wire to motor enable 3
  pinMode(9, OUTPUT); // white wire to motor direction 1
  pinMode(8, OUTPUT); //gray wire to motor direction 2
  pinMode(7, OUTPUT); //purple wire to motor direction 3
  pinMode(6, INPUT); //orange encoder from motor 1
  pinMode(5, INPUT); //red encoder from motor 1
  pinMode(4, INPUT); //brown encoder from motor 1
  pinMode(ledPin, OUTPUT);   // initialize the LED pin as an output:     
  pinMode(buttonPin, INPUT); // initialize the pushbutton pin as an input:    
}

void loop(){
  while (headPush == 0) {
  // wait for head push before starting
     buttonState = digitalRead(buttonPin);
     if (buttonState == HIGH) {     
        // turn LED on:    
        digitalWrite(ledPin, HIGH);  
        headPush = 1;
     }
  } 
//  oneMotorAtATime();
//  spinTightCCW();
  oneMotorAtATime();
  delay(1000);
  spinTightCCW();
  delay(1000);
  spinTightCW();
  delay(1000);
  gradualTurn();
  delay(1000);
  forWard();
  delay(1000);
  backWard();
  delay(1000);
  rightForward();
  delay(1000);
  rightBackward();
  delay(1000);
  headPush=0;  
}

void oneMotorAtATime() {

  //motors turning individually
  delay(1000);              // wait for a second
  digitalWrite(12, HIGH);   // set the LED on
  delay(2000);              // wait for a second
  digitalWrite(12, LOW);    // set the LED off
  delay(2000);              // wait for a second
  digitalWrite(11, HIGH);   // set the LED on
  delay(2000);              // wait for a second
  digitalWrite(11, LOW);    // set the LED off
  delay(2000);              // wait for a second
  digitalWrite(10, HIGH);   // set the LED on
  delay(2000);              // wait for a second
  digitalWrite(10, LOW);    // set the LED off
  headPush=0;
}

void spinTightCCW() {
  delay(1000);
  digitalWrite(9, HIGH);   // reverse motor 1
  digitalWrite(8, HIGH);   // reverse motor 2
  digitalWrite(7, HIGH);   // reverse motor 3
  delay(100);
  digitalWrite(12, HIGH);   // turn motor 1 on 
  digitalWrite(11, HIGH);   // turn motor 1 on
  digitalWrite(10, HIGH);   // turn motor 3 on
  delay(840);
  digitalWrite(12, LOW);   // turn motor 1 off 
  digitalWrite(11, LOW);   // turn motor 2 off
  digitalWrite(10, LOW);   // turn motor 3 off
  digitalWrite(9, LOW);   // normal motor 1
  digitalWrite(8, LOW);   // normal motor 2
  digitalWrite(7, LOW);   // normal motor 3 
  headPush=0;  
}

void spinTightCW() {
  delay(1000);
  digitalWrite(12, HIGH);   // turn motor 1 on 
  digitalWrite(11, HIGH);   // turn motor 1 on
  digitalWrite(10, HIGH);   // turn motor 3 on
  delay(840);
  digitalWrite(12, LOW);   // turn motor 1 off 
  digitalWrite(11, LOW);   // turn motor 2 off
  digitalWrite(10, LOW);   // turn motor 3 off
  headPush=0;  
}


void forWard() {
  delay(1000);
  digitalWrite(12, HIGH);   // turn motor 1 on 
  digitalWrite(8, HIGH);   // reverse direction of motor 2
  digitalWrite(11, HIGH);   // turn motor 2 on
  delay(2000);
  digitalWrite(12, LOW);   // turn motor 1 off
  digitalWrite(8, LOW);   // make direction of motor 2 normal
  digitalWrite(11, LOW);   // turn motor 2 off
  headPush=0;
}

void backWard() {
  delay(1000);
  digitalWrite(9, HIGH);   // reverse direction of motor 1
  digitalWrite(12, HIGH);   // turn motor 1 on   
  digitalWrite(11, HIGH);   // turn motor 2 on
  delay(2000);
  digitalWrite(12, LOW);   // turn motor 1 off
  digitalWrite(9, LOW);   // make direction of motor 2 normal
  digitalWrite(11, LOW);   // turn motor 2 off
  headPush=0;
}

void rightForward() {
  delay(1000);
  digitalWrite(11, HIGH);   // turn motor 1 on 
  digitalWrite(7, HIGH);   // reverse direction of motor 2
  digitalWrite(10, HIGH);   // turn motor 2 on
  delay(2000);
  digitalWrite(11, LOW);   // turn motor 1 off
  digitalWrite(7, LOW);   // make direction of motor 2 normal
  digitalWrite(10, LOW);   // turn motor 2 off
  headPush=0;
}

void rightBackward() {
  delay(1000);
  digitalWrite(11, HIGH);   // turn motor 1 on 
  digitalWrite(8, HIGH);   // reverse direction of motor 2
  digitalWrite(10, HIGH);   // turn motor 2 on
  delay(2000);
  digitalWrite(11, LOW);   // turn motor 1 off
  digitalWrite(8, LOW);   // make direction of motor 2 normal
  digitalWrite(10, LOW);   // turn motor 2 off
  headPush=0;
}

void gradualTurn() {
  delay(1000);
  digitalWrite(12, HIGH);   // turn motor 1 on 
  digitalWrite(8, HIGH);   // reverse direction of motor 2
  digitalWrite(11, HIGH);   // turn motor 2 on
  delay(200);
  digitalWrite(10,HIGH); // turn motor 3 on
  delay(200);
  digitalWrite(10,LOW); // turn motor 3 on
  delay(200);
  digitalWrite(10,HIGH); // turn motor 3 on
  delay(200);
  digitalWrite(10,LOW); // turn motor 3 on
  delay(200);
  digitalWrite(10,HIGH); // turn motor 3 on
  delay(200);
  digitalWrite(10,LOW); // turn motor 3 on
  delay(200);
  digitalWrite(10,HIGH); // turn motor 3 on
  delay(200);
  digitalWrite(10,LOW); // turn motor 3 on
  delay(200);  
  digitalWrite(12, LOW);   // turn motor 1 off
  digitalWrite(8, LOW);   // make direction of motor 2 normal
  digitalWrite(11, LOW);   // turn motor 2 off
  headPush=0; 
}