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

CMindstorms robot;

double powerFactor = 0.5;  // an arbitrary constant for calibrating output power
double turnLeft,  // correction value of left motor power
       turnRight;  // correction value of right motor power
double targetPower = 15;  // power that outputs when the difference of lightValueLeft and lightValueRight is 0

int lightValueLeft,  // value of the left light sensor reading
    lightValueRight;  // value of the right light sensor reading
int powerLeft,
    powerRight;
    
robot.setSensorLight(PORT3, "Reflect");
robot.setSensorLight(PORT2, "Reflect");

/* an infinite while-loop which keeps the robot running */
while (1) {
    robot.getSensorLight(PORT3, lightValueLeft);
    robot.getSensorLight(PORT2, lightValueRight);

    /* an if-statement to determine if the robot is at the edge of an area */
    if (lightValueLeft < 55 || lightValueRight < 55) {
        turnLeft = (lightValueRight - lightValueLeft)*powerFactor;
        turnRight = (lightValueLeft - lightValueRight)*powerFactor;
        
        powerLeft = round(targetPower + turnLeft);
        powerRight = round(targetPower + turnRight);
        
        /* set both the motor power to negative values so the robot goes backward */
        robot.setJointPower(JOINT2, -powerLeft);
        robot.setJointPower(JOINT3, -powerRight);
        
        robot.moveForeverNB();
        
        robot.delaySeconds(1);  // robot moves back for 1 second
    }
    else {
        robot.setJointPower(JOINT2, targetPower);
        robot.setJointPower(JOINT3, targetPower);
        
        robot.moveForeverNB();
    }
}