RosieCar

/*
This is a test sketch for the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control

For use with the Adafruit Motor Shield v2
---->   http://www.adafruit.com/products/1438
*/

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);

// Select which 'port' M1, M2, M3 or M4. In this case, M1
int numPointers = 4;


const int REAR_LEFT = 3;
const int REAR_RIGHT = 0;
const int FRONT_LEFT = 2;
const int FRONT_RIGHT = 1;

const int TURN_TIME = 100;
const int MOVE_TIME = 1000;
const int STOP_TIME = 4000;



/*
 * PIR sensor tester
 */

const int PIR_INPUT_PIN = 2;               // choose the input pin (for PIR sensor)
const int PIR_LED_PIN = 13;                // choose the pin for the LED

int val = 0;                    // variable for reading the pin status
int pirState = LOW;             // we start, assuming no motion detected


Adafruit_DCMotor *myMotor[] = {AFMS.getMotor(1),
AFMS.getMotor(2),
AFMS.getMotor(3),
AFMS.getMotor(4)};


void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");

  pinMode(PIR_INPUT_PIN, INPUT);     // declare sensor as input
  pinMode(PIR_LED_PIN, OUTPUT);      // declare LED as output


  AFMS.begin();  // create with the default frequency 1.6KHz
}


void go_forward() {
  Serial.print("FORWARDS!!!!\n");

  // Go forward
  myMotor[REAR_LEFT]->setSpeed(255);
  myMotor[REAR_LEFT]->run(FORWARD);

  myMotor[REAR_RIGHT]->setSpeed(255);
  myMotor[REAR_RIGHT]->run(FORWARD);

  // Go forward
  myMotor[FRONT_LEFT]->setSpeed(255);
  myMotor[FRONT_LEFT]->run(FORWARD);

  myMotor[FRONT_RIGHT]->setSpeed(255);
  myMotor[FRONT_RIGHT]->run(FORWARD);

   delay(MOVE_TIME);
};

void go_backwards() {
  Serial.print("BACKWARDS!!!!\n");

  // Go BACKWARD
  myMotor[REAR_LEFT]->setSpeed(255);
  myMotor[REAR_LEFT]->run(BACKWARD);

  myMotor[REAR_RIGHT]->setSpeed(255);
  myMotor[REAR_RIGHT]->run(BACKWARD);

  myMotor[FRONT_LEFT]->setSpeed(255);
  myMotor[FRONT_LEFT]->run(BACKWARD);

  myMotor[FRONT_RIGHT]->setSpeed(255);
  myMotor[FRONT_RIGHT]->run(BACKWARD);

  delay(MOVE_TIME);
};


void stop() {
     Serial.print("STOP!!!!\n");
     //stop
     myMotor[0]->setSpeed(0);
     myMotor[0]->run(FORWARD);

     myMotor[1]->setSpeed(0);
     myMotor[1]->run(FORWARD);

     myMotor[2]->setSpeed(0);
     myMotor[2]->run(FORWARD);

     myMotor[3]->setSpeed(0);
     myMotor[3]->run(FORWARD);
     delay(STOP_TIME);

}
void go_left() {
  Serial.print("LEFT!!!!\n");

  // Go forward
  myMotor[REAR_LEFT]->setSpeed(255);
  myMotor[REAR_LEFT]->run(BACKWARD);

  myMotor[FRONT_LEFT]->setSpeed(255);
  myMotor[FRONT_LEFT]->run(BACKWARD);

  myMotor[REAR_RIGHT]->setSpeed(255);
  myMotor[REAR_RIGHT]->run(FORWARD);


  myMotor[FRONT_RIGHT]->setSpeed(255);
  myMotor[FRONT_RIGHT]->run(FORWARD);

  delay(TURN_TIME);

};
void go_right() {
  Serial.print("RIGHT!!!!\n");

  // Go forward
  myMotor[REAR_LEFT]->setSpeed(255);
  myMotor[REAR_LEFT]->run(FORWARD);
  myMotor[FRONT_LEFT]->setSpeed(255);
  myMotor[FRONT_LEFT]->run(FORWARD);

  myMotor[REAR_RIGHT]->setSpeed(255);
  myMotor[REAR_RIGHT]->run(BACKWARD);
  myMotor[FRONT_RIGHT]->setSpeed(255);
  myMotor[FRONT_RIGHT]->run(BACKWARD);

  delay(TURN_TIME);
};


void loop() {
  go_left(); // go left a littlee
  disable_pir_sensor();
  stop();    // stop for 4.5 seconds

  while (pir_scan_once()) {
    // Keep moving forward as long as motion is detected ahead
  }
}

void disable_pir_sensor() {
      digitalWrite(PIR_LED_PIN, LOW); // turn LED OFF
      Serial.println("Motion ended!");
      pirState = LOW;
}

boolean pir_scan_once() {

  val = digitalRead(PIR_INPUT_PIN);  // read input value
  if (val == HIGH) {            // check if the input is HIGH
    digitalWrite(PIR_LED_PIN, HIGH);  // turn LED ON
    if (pirState == LOW) {
      // we have just turned on
      Serial.println("Motion detected!");
      // We only want to print on the output change, not state
      pirState = HIGH;

      // If movement was detected, we want to go forward, then stop.
      go_forward();
      stop();
      // After stopping, we go back to the main loop which goes left a little
      // then stops for 4.5 seconds

      // Deactive the PIR
      pirState = LOW;
      digitalWrite(PIR_LED_PIN, LOW);
      return true;
    }
  } else {
    digitalWrite(PIR_LED_PIN, LOW); // turn LED OFF
    if (pirState == HIGH){
      // we have just turned of
      Serial.println("Motion ended!");
      // We only want to print on the output change, not state
      pirState = LOW;
    }
  }
  return false;
}

/* hahaha cx */