wall avoidance program for Arcbotics Sparki.

wallAvoidance.ino

#include <Sparki.h>
 
const byte LEFT = 0;
const byte RIGHT = 1; 
 
class WallAvoidance{
 
  int distError; 
  int distTooClose;
  int distBack; 
  int wait;
  int angle;
  int turnDir;
  
  public:
    WallAvoidance(int vDistTooClose, int vDistBack, int vAngle, byte vTurnDir, int vWait){
      
      distError= -1;
      distTooClose= vDistTooClose;
      distBack = vDistBack;
      wait = vWait;
      angle = vAngle;
      turnDir = vTurnDir;          
    }
    
    boolean positionError(){
      return sparki.ping() == -1 ? true: false;
    }
    
    void errorSignalOn(){
      sparki.beep();
      digitalWrite(STATUS_LED, HIGH);
    }
    
    void errorSignalOff(){
      digitalWrite(STATUS_LED, LOW);
    }
    
    void errorCheck(){
      sparki.servo(SERVO_CENTER);
      while (true){
        if (positionError()){
          errorSignalOn();
          sparki.moveForward();
          delay(wait);
        }else{
          errorSignalOff();
          break;
        }
      }
    }
    
    boolean tooClose(){
      return sparki.ping() < distTooClose ? true : false;
    }
    
    void tooCloseSignal(){
      sparki.beep();
      sparki.RGB(RGB_RED);
    }
    
    void turn(){
      turnDir == LEFT ? sparki.moveLeft(angle) : sparki.moveRight(angle); 
    }
    
    void forwardSignal(){
      sparki.RGB(RGB_GREEN);
    }
    
    void avoid(){
      tooCloseSignal();
      sparki.moveBackward(distBack);
      turn();
    }
    
    void act(){
        
      errorCheck();   
      if (tooClose()){
        avoid();        
      }
      
      forwardSignal();
      sparki.moveForward();
      delay(wait);
    }  
};
 
WallAvoidance wallAvoid(15, 25, 30, RIGHT, 250);
 
void setup(){
}
 
 
void loop(){
  wallAvoid.act();
}