/* File: lineFollowing_fuzzyLogic.ch
   The NXT robot will follow a line based on the Fuzzy Logic control theory */
#include <mindstorms.h>
#include <math.h>

CMindstorms robot;

double black = 33,  // light sensor reading of color black
       yellow = 71;  // light sensor reading of color yellow
double powerFactor;  // the ratio we want to mutiply to the difference of the light sensor reading and the color black or yellow
double powerMax = 20;  // the desired maximum motor power output

int lightValue;
int powerLeft,
    powerRight;

robot.setSensorLight(PORT3, "Reflect");

powerFactor = (powerMax - 0)/(yellow - black);

/* an infinite while-loop */
while (powerFactor > 0) {
    robot.getSensorLight(PORT3, lightValue);
    
    powerLeft = round((lightValue-black)*powerFactor);
    powerRight = round((yellow-lightValue)*powerFactor);
    
    robot.setJointPower(JOINT3, powerLeft);
    robot.setJointPower(JOINT2, powerRight);
    
    robot.moveForeverNB();
}