/* File: objectAvoidance.ch
   the NXT robot will avoid wandering off a specific area */
#include <mindstorms.h>
#include <math.h>

CMindstorms robot;

double powerFactor = 2.4;  // an arbitrary constant for calibrating output power
double offset = 25;  // the closest distance the robot is allowed to move straight forward
double ultraValue;  // value of the ultrasonic sensor reading
double error;  // difference between the current distance from objects and the offset value
double targetPower = 20;  // power that outputs when the difference between lightValueLeft and lightValueRight is 0
double turn;  // correction value for motor power

int powerLeft,
    powerRight;

robot.setSensorUltrasonic(PORT4, "Centimeter");

/* an infinite while-loop which keeps the robot running */
while (powerFactor > 0) {
    robot.getSensorUltrasonic(PORT4, ultraValue);
    
    /* an if-statement to determine if the robot takes action to avoid hitting objects */
    if (ultraValue > offset) {
        robot.setJointPower(JOINT3, targetPower);
        robot.setJointPower(JOINT2, targetPower);
        
        robot.moveForeverNB();
    }
    else {
        error = ultraValue - offset;
        turn = error*powerFactor;
        
        powerLeft = round(targetPower + turn);
        powerRight = round(targetPower - turn);
        
        robot.setJointPower(JOINT3, powerLeft);
        robot.setJointPower(JOINT2, powerRight);
        
        robot.moveForeverNB();
    }
}