Update 'main.c'

main.c

@@ -33,19 +33,61 @@
 #define TILE 1206
 
 // How much the wheels should spin in a 90 degree turn
-#define POINTS_PER_TURN 360 
+#define POINTS_PER_TURN 320 
 
 // definitions for driveTiles()
 #define FORWARD true
 #define REVERSE false
 
+void clearEnc() { // Reset driving motor encoder values to 0
+    nMotorEncoder[driveRB] = 0;
+    nMotorEncoder[driveLB] = 0;    
+}
+
 void shootBall() {
   motor[shoot] = MAX_SPEED;
   wait(1.25); // Shooting takes 1.25 seconds. 
   // Any unwanted extra movement will be undone by the rubber bands.
   motor[shoot] = STOP;
 }
-void clearEnc();
+void turntoRight(float turns) {
+	clearEnc();
+  while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
+    motor[driveLB] = 100;
+    motor[driveLF] = 100; 
+    motor[driveRB] = -100;
+    motor[driveRF] = -100;
+  }
+  motor[driveLB] = 0;
+  motor[driveLF] = 0; 
+  motor[driveRB] = 0;
+  motor[driveRF] = 0;
+}
+void turntoLeft(float turns) {
+  clearEnc();
+	while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
+    motor[driveLB] = -100;
+    motor[driveLF] = -100; 
+    motor[driveRB] = 100;
+    motor[driveRF] = 100;
+  }  
+  motor[driveLB] = 0;
+  motor[driveLF] = 0; 
+  motor[driveRB] = 0;
+  motor[driveRF] = 0;
+}
+      
+
+void flipOn() {
+  motor[bintake] = -MAX_SPEED;
+}
+void ballOff() {
+  motor[bintake] = 0;
+}
+void ballIn() {
+  motor[bintake] = MAX_SPEED;
+}
+
 void joystickDrive() {
   if(abs(vexRT[Ch3]) > DEADZONE) {
     motor[driveLB] = vexRT[Ch3];
@@ -89,17 +131,17 @@ void pre_auton() {
   bStopTasksBetweenModes = true;
 }
 
-void driveTiles(int numberOfTiles, bool direction) { 
+void driveTiles(float numberOfTiles, bool direction) { 
     // when direction is true, move forward, otherwise go in reverse
 	clearEnc();
-	while(direction == FORWARD && numberOfTiles * TILE > nMotorEncoder[1]) {
+	while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) {
-			if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
+			if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) {
     		motor[driveLB] = 100;
     		motor[driveLF] = 100; 
       	motor[driveRB] = 90;
     		motor[driveRF] = 90;
     	}
-      if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
+      if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveRB]) {
     		motor[driveLB] = 90;
     		motor[driveLF] = 90; 
       	motor[driveRB] = 100;
@@ -111,14 +153,14 @@ void driveTiles(int numberOfTiles, bool direction) {
     		motor[driveRF] = 100;
     	}
 	}
-    while(direction == REVERSE && numberOfTiles * TILE > nMotorEncoder[1]) {
+    while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) {
-			if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
+			if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) {
     		motor[driveLB] = -100;
     		motor[driveLF] = -100; 
       	motor[driveRB] = -90;
     		motor[driveRF] = -90;
     	}
-      if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
+      if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveLB]) {
     		motor[driveLB] = -90;
     		motor[driveLF] = -90; 
       	motor[driveRB] = -100;
@@ -136,55 +178,44 @@ void driveTiles(int numberOfTiles, bool direction) {
   motor[driveRF] = 0;
 }
 task autonomous() {
+	turntoRight(0.05);
 	shootBall();
+	turntoLeft(0.05);
 	driveTiles(2, FORWARD); // Move 2 forward to hit bottom flag
   driveTiles(1, REVERSE);
-  turnRight(1);
+  turntoRight(1);
-  driveTiles(0.33, REVERSE); // Drive 1/3 of a tile backwards to hit the wall and align ourselves!
+  driveTiles(0.5, REVERSE); // Drive 1/3 of a tile backwards to hit the wall and align ourselves!
   flipOn(); // Turn on the ball intake in reverse, which is what we can use to flip the caps
-  driveTiles(1.25, FORWARD); // flip cap
+  driveTiles(1.5, FORWARD); // flip cap
   ballOff();
+  driveTiles(1.25, REVERSE);
+  turntoLeft(1);
   driveTiles(1, REVERSE);
-  turnLeft(1);
+  turntoRight(1);
-  driveTiles(1, REVERSE);
+  driveTiles(0.6, REVERSE);
-  turnRight(1);
-  driveTiles(0.33, REVERSE);
   driveTiles(2.5, FORWARD); // Flip the other cap without turning on the spinner
-  ballIn(); // So we can pick up the ball!
+  flipOn();
-  delay(1000);
+  driveTiles(0.5, FORWARD);
   driveTiles(0.1, FORWARD);
-  turnLeft(1);
+  ballIn(); // So we can pick up the ball!
-  driveTiles(0.33, REVERSE);
+  wait(3);
-  delay(1000);
+  driveTiles(0.1, FORWARD);
+  turntoLeft(1);
+  driveTiles(0.25, REVERSE);
+  wait(3);
   ballOff();
   shootBall();
-  driveTiles(2.2, FORWARD); // Hit middle column bottom flag
+  driveTiles(0.25, FORWARD); // Hit middle column bottom flag
+	//driveTiles(2, REVERSE);
+	turntoRight(1);
+	driveTiles(2.5, REVERSE);
+	turntoLeft(1);
+	driveTiles(1, REVERSE);
+	turntoRight(1);
+	driveTiles(0.25, REVERSE);
+	driveTiles(3, FORWARD);
 	
 }
-void turnRight(int turns) {
-  while(turns * POINTS_PER_TURN < nMotorEncoder[1]){
-    	
-  }
-}
-void turnLeft(int turns) {
-  while(turns * POINTS_PER_TURN < nMotorEncoder[1]){
-    	
-  }  
-}
-      
-void clearEnc() { // Reset driving motor encoder values to 0
-    nMotorEncoder[1] = 0;
-    nMotorEncoder[2] = 0;    
-}
-void flipOn() {
-  motor[bintake] = -MAX_SPEED;
-}
-void ballOff() {
-  motor[bintake] = 0;
-}
-void ballIn() {
-  motor[bintake] = MAX_SPEED;
-}
 task usercontrol() { // In user control mode
   while (true) {
     joystickDrive(); // Joystick mapping function