#pragma config(Sensor, dgtl1,  quad,                sensorQuadEncoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task clutch(){
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  SensorValue(quad) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true){
    if(motor[port2] > 20 || motor[port2] < -20){ //checks if motor is running at a reasonable speed
      wait1Msec(100); //checks for stop and go movement
      while(motor[port2] > 20 || motor[port2] < -20){
        quadPrev = SensorValue(quad); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - SensorValue(quad)) < (abs(motor[port2]) * threshold)){
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[port2] = 0; // stops motor
          hogCPU(); //stops task main from executing until the release button is pressed
          while(!vexRT[Btn6U]){ //this button releases the motor
            motor[port2] = 0; //stops the motor which is not running correctly
            //insert all user controls except for the motor that is being stopped
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}

task main(){
  StartTask(clutch);
  while(true){
  motor[port2] = vexRT[Ch1];
    wait1Msec(1);//needed for multitasking to work correctly
  }
}