From 5d3e755cc37dea817b08e88f4697275b927c56b8 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Tue, 15 Dec 2015 16:28:26 -0500
Subject: autonomizing robot

---
 src/auto.c | 49 ++++++++++++++++++-------------------------------
 1 file changed, 18 insertions(+), 31 deletions(-)

(limited to 'src/auto.c')

diff --git a/src/auto.c b/src/auto.c
index 2332428..a5bbb5d 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,41 +1,28 @@
 #include <main.h>
 
-/*#define TARGET_SPEED 65
-
-static unsigned int time;
-
-void autoDelay(unsigned int ms){
-	delay(ms);
-	time-=ms;
-}*/
+#define TARGET_RPM 1700
 
 void autonomous(){
-	/*static char speed = 0;
+	/*static double cl,cr,ca;
+	static char speed;
 
-	time = 15000;
+	speed = 30;
 
 	do{
-
-		speed+=10;
-
-		motorSet(CANNON1,speed);
-		motorSet(CANNON2,speed);
-		motorSet(CANNON3,speed);
-		motorSet(CANNON4,speed);
-
-		autoDelay(200);
-
-	}while(speed<TARGET_SPEED);
-
-	autoDelay(800);
-
-	motorSet(LIFT1 ,127);
-	motorSet(LIFT2 ,127);
-	motorSet(INTAKE,127);
-
-	//while(1);
-	delay(time);
+		speed += 10;
+		zMotorSet("Left cannon",-speed);
+		zMotorSet("Right cannon",speed);
+		delay(800);
+		cl =  zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;
+		cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;
+		ca = (cl + cr) / 2;
+	}while(ca < TARGET_RPM);
+
+	zMotorSet("Misc",127);
+	delay(900);
+	zMotorSet("Misc",-127);
+	delay(900);
+	zMotorSet("Misc",0);
 
 	motorStopAll();*/
-
 }
-- 
cgit v1.2.3