/* Code Developed by 
SCOTT MITCHELL, PhD. RMIT University scott.mitchell@rmit.edu.au
 &
Daud Imran SHAMSUL AMRI www.imranshamsul.com / imran.shamsul@gmail.com
 
 DATE: 22 OCT 2012
 
 Code for custom design pcb.
 Code functions include: 
 
 - Pot to control max distance (min distance set to 2cm so motor will not power on when set)
 - Power Sensor via Pin
 - Button, when motor off, sensor off.
 - second pot to control PWM output
 
 
 
 */

#include "Ultrasonic.h"

#include "Button.h"

Ultrasonic sensor1(9,10); // Ultrasonic(int TP, int EP);
const int MOT1 = 11; //D11 PWM
const int distPotPin = 6; //A6 distance pot
const int pwrPotPin = 7; //A7 power pot
const int sensor1Power = 8; // D8 PWM8

//distance min and max
const int minDistance = 2;  //dist in CM dont go below 2
const int maxDistance = 200;  //dist in CM

//power min and max
const int minPower = 0;
const int maxPower = 255;

int motorPWM, distPot, distControl; 
int outputPWM, pwrPot, pwrControl; 

long cm; 

boolean motorState = true;

Button button1 = Button(2,PULLUP);  //button control



void setup() {
  pinMode(MOT1, OUTPUT);
  pinMode(sensor1Power, OUTPUT);
  digitalWrite(sensor1Power, HIGH);

  Serial.begin(9600); // this just means you can output to the serial panel
}

void loop()
{ 
  if(button1.uniquePress()){ 
    motorState = !motorState;
  }

  button1.isPressed();

  // check the distance pot
  distPot = analogRead(distPotPin);
  distControl = map(distPot,0,1024,minDistance,maxDistance);


 //check the power pot
  pwrPot = analogRead(pwrPotPin); 
  pwrControl = map(pwrPot,0,1024,minPower,maxPower);


  if(motorState){
    cm = sensor1.Ranging(CM);

    motorPWM = map(cm,distControl,minDistance,minPower,pwrControl); //variable map formula relationship
    motorPWM = constrain(motorPWM, 0, 255);

    analogWrite(MOT1, motorPWM); 
    digitalWrite(sensor1Power, HIGH);
  }
  
  else {
    analogWrite(MOT1, 0);
    digitalWrite(sensor1Power, LOW);
  }


  Serial.print(cm);
  Serial.print("cm Max ");
  Serial.print(distControl);
  Serial.print("cm motorState:");
  Serial.print(motorState);
  Serial.print(" pwr control::");
   Serial.print(pwrControl);
 Serial.print(" PWM ");
  Serial.println(motorPWM);

  // delay(50);
}