Everything Combined 5-21-19

#pragma config(Sensor, in2,    ArmLF,          sensorLineFollower)
#pragma config(Sensor, in3,    CDLF,           sensorLineFollower)
#pragma config(Sensor, dgtl1,  distance,       sensorSONAR_cm)
#pragma config(Sensor, dgtl3,  eStop,          sensorTouch)
#pragma config(Sensor, dgtl4,  en,             sensorQuadEncoder)
#pragma config(Motor,  port2,           mGrip,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           mSlider,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           mRotate,       tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           mm,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           lidStamper,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           pmCyl,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           chestClose,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           coinStamper,   tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//mm is the cd racks motor
//cm is the counters motor
//km is the lid closer motor

int counter = 0;
int coinsPassed = 0;
int cdRotations = 0;

task e_stop()
{
  while(true)
  {
    if(SensorValue(eStop) == 1)
    {
    	stopAllTasks();      // ends the program and all tasks including task main.
    }
    	wait1Msec(10);  	// prevents the current task from using majority of available CPU capacity
  }
}
task cdCounter()
{
	while(true) {
		if(SensorValue(CDLF) < 1800 && SensorValue(CDLF) > 1400) {
			coinsPassed = coinsPassed + 1;
			waitUntil(SensorValue(CDLF) > 1800); //Wait until coin passes
		}

		if(coinsPassed == 4) {
			coinsPassed = 0;
		}

		wait1Msec(5);
	}
}
bool goTo(tMotor m, float point, int speed)
{
	while(SensorValue(en) != point)
	{
		if(SensorValue(en) > point)
		{
			startMotor(m, -1 * speed);
		}
		else
		{
			startMotor(m, speed);
		}
	}
	stopMotor(m);
	return true;
}

/* Will attempt to go to an exact point 6 times
If after 6 attempts it can't get the exact value requested it will
accept a value that is +- 1 of the requested value */
bool goToExactValue(tMotor m, float point)
{
	int completed = false;
	int fixAttempts = 0; //Here so robot doesn't get stuck and try to correct itself forever

	while(!completed) {
		waitUntil(goTo(m, point, 19));
		wait(.25);  //Allow the motor to fully stop before correction
		if(SensorValue(en) != point  && fixAttempts <= 5) {
			waitUntil(goTo(m, point, 19));
			fixAttempts = fixAttempts + 1;
		} else {
			completed = (SensorValue(en) == point || SensorValue(en) == point + 1 || SensorValue(en) == point - 1);
		}
	}

	return true;
}

bool manipulateGrip(bool open) {
	if(open) {
		startMotor(mGrip, 40);
		} else {
		startMotor(mGrip, -40);
	}

	wait(.5);
	stopMotor(mGrip);
	return true;
}

bool moveSlider(float point)
{
	if(SensorValue(distance) > point) {
		startMotor(mSlider, -25);
		waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
		startMotor(mSlider, 25);
		waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
	} else {
		startMotor(mSlider, 25);
		waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
		startMotor(mSlider, -25);
		waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
	}
	stopMotor(mSlider);

	return true;
}

bool rotateArm(bool home) {
	if (home) {
		setServo(mRotate, -15);
		} else {
		setServo(mRotate, -120);
	}

	wait(1);

	return true;
}

task cdRack()
{
	SensorValue(en) = 0;
	//startMotor(cm, 20);
	startTask(cdCounter);

	while(true) {
		if(coinsPassed == 4) {
			//Wiggle before moving
			waitUntil(goTo(mm, (90 * cdRotations) + 10, 100));
			waitUntil(goTo(mm, (90 * cdRotations) - 10, 100));
			cdRotations = cdRotations + 1;
			waitUntil(goToExactValue(mm, 90 * cdRotations));// No longer using the gear makes corrections not needed
			//waitUntil(goTo(mm, 90 * cdRotations, 30));
		}
	}
}

task main()
{
	///startTask(e_stop);
	startTask(cdRack);

	waitUntil(manipulateGrip(true)); //open gripper
	waitUntil(rotateArm(true)); //Move the arm to the home position

	while(true) {
		if(SensorValue(ArmLF) < 2800 && counter <= 3) {
			waitUntil(manipulateGrip(false)); //close gripper
			int position = (counter * 3) + 6;
			waitUntil(rotateArm(false));//Rotate the arm
			waitUntil(moveSlider(position)); //Move slider to position
			counter = counter + 1;

			waitUntil(manipulateGrip(true)); //open the gripper
			wait(2);
			waitUntil(moveSlider(6)); //Move slider back home
			waitUntil(rotateArm(true)); //Move arm back to home

			if(counter == 3) {
				counter = 0;
			}
		}
		waitInMilliseconds(50);
	}

}