/*

Little Helper v.SRF02

Sensor connections:
 SDA - Analog pin 4
 SCL - Analog pin 5
 
*/

#include <Wire.h>

#define sensorAddress 0x70
#define readCentimeters 0x51
#define resultRegister 0x02
#define transistorPin 2
#define ledPin 13
#define analogInput 3
int analogValue;

void setup() {
  Wire.begin();
  // Serial.begin(19200);
  pinMode(transistorPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
}

void loop()
{  

 sendCommand(sensorAddress, readCentimeters); 
  delay(70);
  setRegister(sensorAddress, resultRegister);
  int sensorReading = readData(sensorAddress, 2);
  analogValue = analogRead(analogInput) / 2;
  
  /*
  Serial.print('A');
  Serial.println(sensorReading);
  Serial.print('B');
  Serial.println(analogValue);
  delay(70);
  */
  
  if (sensorReading < analogValue) { // Set activation threshold in cm
  
  /* for loop adds a "tail" to the motors activation, so that movement continues
     for a short time after it has been activated.
     remove for loop to have direct maping of ultrasoundValue onto motor speed. */
     
      for (int i=0; i < random(60, 140); i++) {
          digitalWrite(transistorPin, LOW); // turn motor off.
          digitalWrite(ledPin, LOW); 
          delay(random(5, 25));
          digitalWrite(transistorPin, HIGH); // turn motor on;
          digitalWrite(ledPin, HIGH);
          delay(10);
      }
}

  else {
      digitalWrite(transistorPin, LOW); // turn motor off.
      digitalWrite(ledPin, LOW); 
      //delay(100);
    }

  
}

void sendCommand(int address, int command) {
  Wire.beginTransmission(address);
  Wire.send(0x00);
  Wire.send(command);
  Wire.endTransmission();
}

void setRegister(int address, int thisRegister) {
  Wire.beginTransmission(address);
  Wire.send(thisRegister);
  Wire.endTransmission();
}

int readData(int address, int numBytes) {
  int result = 0;
  Wire.requestFrom(address, numBytes);
  while (Wire.available() < 2) {
  }
  
  result = Wire.receive() * 256;
  result = result + Wire.receive();
  return result;
}