//Wormeostat_v1 -- Run this program on both NXT bricks simultaneously.
//The motors feed back to themselves electrically and to each other physically.
#define Motor1 OUT_A
#define Motor2 OUT_B

const int RefreshDelay = 50; //time between sensing rotation and running motor
const int Strength = 80; //motor strength in percent
const int Threshold = 8; // Used to decide to run motor, or not
const int reverse = -1, forward = 1; //a few basic constants


//Motor1=================================================================
int Motor1Response, Motor1Activation, Motor1Sense, Motor1Lag, Motor1Direction;
task Motor1SenseRotation(){
//This task senses the amount that Motor1 has rotated. It records this
//value in a variable (Motor1Sense). This permits more than one task
//to have access to this value at the same time.
while (true){
Motor1Sense = MotorRotationCount(Motor1);
}
}
//Compute movement: Compute rotation required to return to initial position.
task Motor1ComputeRotation(){
while (true){
Motor1Response = (-1 * Motor1Sense); //Response opposite to sensed rotation
//Hold computed value until it is reached or until motor jams
until ((Motor1Response == Motor1Sense) || MotorOverload(Motor1));
Wait(Motor1Lag); // short delay before recomputing
}
}
//React by running the motor as computed by Motor1ComputeRotation.
// Only run the motor if sensed rotation is greater than a threshold
task Motor1React(){
while (true) {
//Set the direction in which to run the motor
if(Motor1Sense > Motor1Response) Motor1Direction = reverse;
else Motor1Direction = forward;
//If sensed rotation is above threshold, run the motor to react
if(abs(Motor1Sense - Motor1Response) > Motor1Activation){
OnFwdReg (Motor1, Motor1Direction * Strength, OUT_REGMODE_SPEED);
}
else Float(Motor1); //If below threshold, do not run the motor
}
}

//Motor2 mirrors Motor1 in all ways.==========================================
int Motor2Response, Motor2Activation, Motor2Sense, Motor2Lag, Motor2Direction;

task Motor2SenseRotation(){
  //This task senses the amount that Motor2 has rotated. It records this
  //value in a variable (Motor2Sense). This permits more than one task
  //to have access to this value at the same time.
  while (true){
    Motor2Sense = MotorRotationCount(Motor2);
  }
}

//Compute movement: Compute rotation required to return to initial position.
task Motor2ComputeRotation(){
  while (true){
     Motor2Response = (-1 * Motor2Sense); //Response opposite to sensed rotation
     //Hold computed value until it is reached or until motor jams
     until ((Motor2Response == Motor2Sense) || MotorOverload(Motor2));
     Wait(Motor2Lag); // short delay before recomputing
  }
}

//React by running the motor as computed by Motor2ComputeRotation.
// Only run the motor if sensed rotation is greater than a threshold
task Motor2React(){
  while (true) {
     //Set the direction in which to run the motor
     if(Motor2Sense > Motor2Response) Motor2Direction = reverse;
     else Motor2Direction = forward;
     //If sensed rotation is above threshold, run the motor to react
     if(abs(Motor2Sense - Motor2Response) > Motor2Activation){
     OnFwdReg (Motor2, Motor2Direction * Strength, OUT_REGMODE_SPEED);
  }
  else Float(Motor2); //If below threshold, do not run the motor
  }
}

//Main================================================================
task main()
{
  // Set Initial Activation Levels and Lag
  Motor1Activation = Threshold;
  Motor1Lag = RefreshDelay;
  Motor2Activation = Threshold;
  Motor2Lag = RefreshDelay;
  //Activate Motor1 Modules
  start Motor1SenseRotation;
  start Motor1ComputeRotation;
  start Motor1React;
  //Activate Motor2 Modules
  start Motor2SenseRotation;
  start Motor2ComputeRotation;
  start Motor2React;
}