/* File: lightFollowing.ch
   the NXT robot will track a light source and move toward it */
#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

int lightValueLeft;  // value of the left light sensor reading
int lightValueRight;  // value of the right light sensor reading
int targetPower = 20;  // power that outputs when the difference of lightValueLeft and lightValueRight is 0
int powerLeft,
    powerRight;
    
robot.setSensorLight(PORT3, "Ambient");
robot.setSensorLight(PORT2, "Ambient");

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

    turnLeft = (lightValueRight - lightValueLeft)*powerFactor;
    turnRight = (lightValueLeft - lightValueRight)*powerFactor;
    
    powerLeft = round(targetPower + turnLeft);
    powerRight = round(targetPower + turnRight);
    
    robot.setJointPower(JOINT3, powerLeft);
    robot.setJointPower(JOINT2, powerRight);
    
    robot.moveForeverNB();
}