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; }