#include <TimedAction.h>
#include <avr/wdt.h> // for watchdog timer function

TimedAction forwardAction = TimedAction(100,moveForward);
TimedAction rangeAction = TimedAction(500,readrange);

void readrange(); // range sensor
void accel(); // accelerometer

int motorPins[] = {2, 3, 4, 5};
int motorPins2[] = {6, 7, 8, 9};
int count = 0;
int count2 = 0;
int delayTime = 4;
int val = 0;
int analogInPin = A0; // pin for range sensor
const int xPin = 9; // x pin for accelerometer
const int yPin = 10; // y pin for accelerometer
float sensorValue; // value of sensor


void setup() {
  Serial.begin(9600); //9600 baud rate

  for (count = 0; count < 4; count++) {
    pinMode(motorPins[count], OUTPUT);
  }

  pinMode(analogInPin, INPUT); // range sensor analog input 0
  pinMode(xPin, INPUT); // accelerometer x digital input 9
  pinMode(yPin, INPUT); // accelerometer y digital input 10

}


void moveBackward() {

  if ((count2 == 0) || (count2 == 1)) {
    count2 = 16;
  }
  count2>>=1;
  for (count = 3; count >= 0; count--) {
    digitalWrite(motorPins[3 - count], count2>>count&0x01);
  }
  delay(delayTime);


}

void readrange(){  // range sensor function
 //forwardAction.disable();

  int temp; // temporary variable for storing value

  temp = analogRead(analogInPin);
  sensorValue = (6787.0 /((float)temp - 3.0)) - 4.0; // conversion to cm
 // approximately 10 to 80 cm

if(sensorValue <= 80 && sensorValue >= 0){
Serial.println(sensorValue);
}
else {
 Serial.println("out of range");
}

delay(300);
 // pass the sensorValue variable to the motor function
//forwardAction.enable();
}

void moveForward() {

  while (sensorValue >= 20){
  if ((count2 == 0) || (count2 == 1)) {
    count2 = 16;
  }
  count2>>=1;
  for (count = 3; count >= 0; count--) {
    digitalWrite(motorPins[count], count2>>count&0x01);
  }
  delay(delayTime);

  }
}

void loop() {
    val = 1000;
    //val = 400;
    //moveForward();
    //readrange(); // range sensor function
    //accel(); // accelerometer function

    forwardAction.check();
    rangeAction.check();

    if (val < 480) {
    moveBackward();

}
}