Jan7CodeCircles

#pragma config(Sensor, dgtl1,  armMax,         sensorDigitalIn)
#pragma config(Motor,  port1,           rightDrive,    tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,           claw,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           arm,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          leftDrive,     tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	int cutoff = 10;
	bool prevDown = false;
	while(1==1)
	{
		bool going = false;
		float width = 10;
	  float multi = 0.1;
		bool sg1 = vexRT[Ch2] > cutoff || vexRT[Ch2] < -cutoff;
		bool sg2 = vexRT[Ch1] > cutoff || vexRT[Ch1] < -cutoff;
		if (sg1 && sg2)
		{

			int x = vexRT[Ch1]*multi;
			int y = vexRT[Ch2]*multi;
	    float a, c, s, area, radius, ratio, sa, sc, sabc, aac, a4, radm, radp;
	    int posneg = 1;

	    if(vexRT[Ch2] < -cutoff)
	    {
	    	posneg = -1;
	    }

			a = sqrt(x*x + y*y);
			c = y * 2;

			s = a + a + c;
	    s /= 2;

	    sa = s - a;
	    sc = s - c;
	    sabc = sa * sa * sc;
	    sabc *= s;

			area = sqrt(s);

			aac = a * a * c;
	    a4 = area * 4;

			radius = aac/a4;
	    radp = radius + width;
	    radm = radius - width;
			ratio = radm/radp;
			float powerless = 80 * ratio * posneg;
			float powermore = 80 * posneg;

			if(vexRT[Ch1] < -cutoff)
			{
				motor[leftDrive] = powerless;
				motor[rightDrive] = powermore;
			}

			else
			{
				motor[leftDrive] = powermore;
				motor[rightDrive] = powerless;
			}

			going = true;
		}
		// THE BELOW TWO THINGS DONT WORK FIX THEM
		else if (vexRT[Ch2] > cutoff)
		{
			motor[leftDrive] = 63.5;
			motor[rightDrive] = 63.5;
			going = true;
		}
		else if (vexRT[Ch2] < -cutoff)
		{
			motor[leftDrive] = -63.5;
			motor[rightDrive] = -63.5;
			going = true;
		}

		if(!going)
		{
			motor[leftDrive] = 0;
			motor[rightDrive] = 0;
		}
		if(vexRT[Btn5U])
		{
			motor[arm] = -63.5;
		}
		else if(vexRT[Btn5D] && SensorValue[armMax] == 1)
		{
			motor[arm] = 63.5;
			prevDown = true;
		}
		else
		{
			if(prevDown)
			{
				motor[arm] = -63.5;
				wait1Msec(35);
				prevDown = false;
			}

			motor[arm] = -10;
		}
		if(SensorValue[armMax] == 0)
		{
			motor[arm] = -63.5;
			wait1Msec(100);
			motor[arm] = 0;
			wait1Msec(500);
		}
		if(vexRT[Btn6U])
		{
			motor[claw] = -63.5;
		}
		else if(vexRT[Btn6D])
		{
			motor[claw] = 63.5;
		}
		else
		{
			motor[claw] = 0;
		}
	}

}