skills and bug fixes

skills auton added and driver controls fixed

Author: Aban0

72832B - Push Bot.c

@@ -1,11 +1,15 @@
 #pragma config(Sensor, dgtl1,  rightDrive,     sensorQuadEncoder)
 #pragma config(Sensor, dgtl3,  leftDrive,      sensorQuadEncoder)
-#pragma config(Motor,  port1,           rightDrive1,   tmotorVex393_HBridge, openLoop, reversed)
-#pragma config(Motor,  port2,           rightDrive2,   tmotorVex393_MC29, openLoop)
-#pragma config(Motor,  port3,           rightDrive3,   tmotorVex393_MC29, openLoop)
-#pragma config(Motor,  port8,           leftDrive1,    tmotorVex393_MC29, openLoop)
-#pragma config(Motor,  port9,           leftDrive2,    tmotorVex393_MC29, openLoop)
-#pragma config(Motor,  port10,          leftDrive3,    tmotorVex393_HBridge, openLoop, reversed)
+#pragma config(Motor,  port1,           mongoLeft,     tmotorVex393_HBridge, openLoop, reversed)
+#pragma config(Motor,  port2,           leftDrive1,    tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port3,           leftDrive2,    tmotorVex393_MC29, openLoop)
+#pragma config(Motor,  port4,           leftDrive3,    tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port5,           rightLift,     tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port6,           leftLift,      tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port7,           rightDrive3,   tmotorVex393_MC29, openLoop)
+#pragma config(Motor,  port8,           rightDrive2,   tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port9,           rightDrive1,   tmotorVex393_MC29, openLoop, reversed)
+#pragma config(Motor,  port10,          mongoRight,    tmotorVex393_HBridge, openLoop)
 //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
 /*---------------------------------------------------------------------------*/
@@ -55,6 +59,70 @@ void pre_auton()
 /*---------------------------------------------------------------------------*/
 
 // Declaring functions for auton
+void moveForward()
+{
+	motor[leftDrive1] = 127;
+	motor[leftDrive2] = 127;
+	motor[leftDrive3] = 127;
+	motor[rightDrive3] = 127;
+	motor[rightDrive2] = 127;
+	motor[rightDrive1] = 127;
+}
+
+// Stop all drive motors
+void stopDrive()
+{
+	motor[leftDrive1] = 0;
+	motor[leftDrive2] = 0;
+	motor[leftDrive3] = 0;
+	motor[rightDrive3] = 0;
+	motor[rightDrive2] = 0;
+	motor[rightDrive1] = 0;
+}
+
+void rotateLeft()
+{
+	motor[leftDrive1] = -127;
+	motor[leftDrive2] = -127;
+	motor[leftDrive3] = -127;
+	motor[rightDrive3] = 127;
+	motor[rightDrive2] = 127;
+	motor[rightDrive1] = 127;
+}
+
+void rotateRight()
+{
+	motor[leftDrive1] = 127;
+	motor[leftDrive2] = 127;
+	motor[leftDrive3] = 127;
+	motor[rightDrive3] = -127;
+	motor[rightDrive2] = -127;
+	motor[rightDrive1] = -127;
+}
+
+void moveBackward()
+{
+	motor[leftDrive1] = -127;
+	motor[leftDrive2] = -127;
+	motor[leftDrive3] = -127;
+	motor[rightDrive3] = -127;
+	motor[rightDrive2] = -127;
+	motor[rightDrive1] = -127;
+}
+
+void mongoUp()
+{
+	motor[mongoRight] = 127;
+	motor[mongoLeft] = 127;
+}
+
+void mongoDown()
+{
+	motor[mongoRight] = -127;
+	motor[mongoLeft] = -127;
+}
+
+
 void blueFlagSide()
 {
 
@@ -77,6 +145,9 @@ void redFarSide()
 
 task autonomous()
 {
+		//flag
+	moveBackward();
+	wait1Msec(1750);
 	// Chooses which auton to run based on where jumper clips are placed
 	if (vexRT[dgtl5] != vexRT[dgtl6] == 1)
 	{
@@ -129,24 +200,67 @@ task usercontrol()
 		motor[leftDrive2] = vexRT[Ch3];
 		motor[leftDrive3] = vexRT[Ch3];
 
-		// PID Parking Brake
+		// Six bar controls
+		if (vexRT[Btn6U] == 1)
+		{
+			motor[rightLift] = 127;
+			motor[leftLift] = 127;
+		}
+		else if (vexRT[Btn6D] == 1)
+		{
+			motor[rightLift] = -127;
+			motor[leftLift] = -127;
+		}
+		else
+		{
+			motor[rightLift] = 0;
+			motor[leftLift] = 0;
+		}
+
+		// Four bar controls
+		if (vexRT[Btn5U] == 1)
+		{
+			motor[mongoRight] = 127;
+			motor[mongoLeft] = 127;
+		}
+		else if (vexRT[Btn5D] == 1)
+		{
+			motor[mongoRight] = -127;
+			motor[mongoLeft] = -127;
+		}
+		else
+		{
+			motor[mongoRight] = 0;
+			motor[mongoLeft] = 0;
+		}
 
+		// PID Parking Brake
 		if (vexRT[Btn8D] == 1)
 		{
 			brakes = !brakes;
 		}
 
 		while (brakes == true)
 		{
+			// PID Parking Brake
+			if (vexRT[Btn8D] == 1)
+			{
+				brakes = !brakes;
+			}
+
 			// Right side
 			errorR = targetSpeedR - SensorValue[rightDrive];  // Currently just P, we can add on in the future
 			drivePowerR += (int)(KpR*errorR);
 			//
 			// Limit the drive power range to 0
 			//
-			if (drivePowerR > 0)
+			if (SensorValue[rightDrive] < drivePowerR)
 			{
-				drivePowerR = 0;
+				while (SensorValue[rightDrive] < drivePowerR)
+				{
+					moveForward();
+				}
+				stopDrive();
 			}
 			else if (drivePowerR < 0)
 			{