class.cpp

#include "motionTracker.h"

MotionTracker::MotionTracker(){
 debugIsOn = false;
 closestSensorIndex = 0;

}

void MotionTracker::setDebugOn(){
  debugIsOn = true; 
}

void MotionTracker::printDebugvalues(){
  Serial.print("sensor one");
  Serial.println(sensorNinty);
  delay(100);
  Serial.print("sensor two:");
  Serial.println(sensorOneEighty);
  delay(100);
}


void MotionTracker::setSensorValues(){
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  sensorNinty = uS / US_ROUNDTRIP_CM;
  unsigned int uSTwo = sonarTwo.ping(); // Send ping, get ping time in microseconds (uS).
  sensorOneEighty = uSTwo / US_ROUNDTRIP_CM;  
  if(debugIsOn){
    printDebugvalues();
  }
  //set sensor 3
  //set sensor 4
}

void MotionTracker::findClosestValue(){
 setSensorValues();
 const int numberSensors = 2;
 int allSensorValues[numberSensors] = {sensorNinty, sensorOneEighty};
 int whereToMoveBasedOnSensor[numberSensors] = {100, 150};
 
 for(int i = 0; i < numberSensors; i++){
   if(allSensorValues[i] < closestSensorValue){
     closestSensorIndex = i;
   }
 }
 if(closestSensorIndex > 0){
   int moveHere = whereToMoveBasedOnSensor[closestSensorIndex];
   moveToActivePositon(moveHere);
 }
}

void MotionTracker::checkActiveSensor(){
  setSensorValues();
  findClosestValue();
  if(sensorNinty > 0){
    triggeredSensor = 100;        
    moveToActivePositon();
  }
  if(sensorOneEighty > 0){
    triggeredSensor = 150;        
    moveToActivePositon();
  }
}

void MotionTracker::moveToActivePositon(int triggeredSensor) {
  if(triggeredSensor == currentPostion){
    return;
  }
  if(triggeredSensor > currentPostion){
    int difference = triggeredSensor - currentPostion;
    motor.step(difference, FORWARD, SINGLE);
    currentPostion =  triggeredSensor; // set active current Position
  }
  else {
    int difference = currentPostion - triggeredSensor;
    motor.step(difference, BACKWARD, SINGLE);
    currentPostion =  triggeredSensor; // set active current Position
  }
}

motionTracker.h

#ifndef MOTIONTRACKER_H
#define MOTIONTRACKER_H

class MotionTracker {
  public:
    MotionTracker();
    void setSensorValues();
    void checkActiveSensor();
    void moveToActivePositon();
    void setDebugOn();
    void printDebugvalues();
  private:
    boolean debugIsOn;

    int triggeredSensor;
    int currentPostion;
    
    void findClosestValue();
    int closestSensorIndex;
    int closestSensorValue;
    
    int sensorNinty;
    int sensorOneEighty;
    //int sensorTwoSeventy;
    //int sensorThreesixty; 
};

#endif