From 476a0dbf41e14458c52a5cd2fe5b4c050a6c6551 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Wed, 13 Jan 2016 16:03:09 -0500
Subject: prog skills

---
 src/auto.c      | 10 +++++-----
 src/opcontrol.c | 36 ++++++++++++++++++++++++++++++------
 2 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/src/auto.c b/src/auto.c
index 5cd39b5..c26bfd9 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,10 +1,10 @@
 #include <main.h>
 #include <zephyr.h>
 
-#define TARGET_RPM 1700
+//#define TARGET_RPM 1700
 
-void autonomous(){
-	/*static double cl,cr,ca;
+/*void autonomous(){
+	static double cl,cr,ca;
 	static char speed;
 
 	speed = 30;
@@ -25,5 +25,5 @@ void autonomous(){
 	delay(900);
 	zMotorSet("Misc",0);
 
-	motorStopAll();*/
-}
+	motorStopAll();
+}*/
diff --git a/src/opcontrol.c b/src/opcontrol.c
index 1d9a0a7..41da208 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -50,6 +50,7 @@ static char cann = 0;
 static double rpm  = 0,
 			  trpm = 1850,
 			  arpm = 0;
+static bool cannReady = false;
 
 /**
  * Contains the current X and Y position in inches. The X axis extends from
@@ -321,8 +322,8 @@ void moveProc(void *unused_param){
 		 * inches per millisecond
 		 */
 
-		lv =  zMotorIMEGetVelocity("Left drive")  / 39.2L * 8.64L / 60000;
-		rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 8.64L / 60000;
+		lv =  zMotorIMEGetVelocity("Left drive")  / 39.2L * 12.566L / 60000;
+		rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000;
 
 		/**
 		 * Get the distance thing.
@@ -388,7 +389,7 @@ void aimProc(void *procPtr){
 		 */
 
 		cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);
-		rangle = zGyroGet() - (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);
+		rangle = zGyroGet() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);
 
 		lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);
 
@@ -422,7 +423,7 @@ void aimProc(void *procPtr){
 
 void cannonProc(void *procPtr){
 	static double cl,cr,ca;
-	static int speed;
+	static int speed;//,ispeed = 0;
 
 	cannonProcRun = true;
 
@@ -524,10 +525,16 @@ void cannonProc(void *procPtr){
 			speed += 2;
 			zMotorSet("Left cannon" ,-speed,2);
 			zMotorSet("Right cannon", speed,2);
+			cannReady = false;
 		}else if(ca > trpm + 40){
 			speed -= 2;
+			//if(speed < ispeed) speed = ispeed;
 			zMotorSet("Left cannon" ,-speed,2);
 			zMotorSet("Right cannon", speed,2);
+			cannReady = false;
+		}else{
+			cannReady = true;
+			//ispeed = speed;
 		}
 
 		lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);
@@ -536,7 +543,6 @@ void cannonProc(void *procPtr){
 		delay(100);
 	}
 
-	zMotorSet("Left cannon" ,0,2);
 	zMotorSet("Right cannon",0,2);
 
 	zMotorReturn("Left cannon");
@@ -633,7 +639,6 @@ PUSH:
  */
 
 void lcdUpdateFunc(void *unused_param){
-	unsigned long elapsed;
 	while(1){
 		/*
 		 * Track elapsed time since operatorControl() entrance.
@@ -647,3 +652,22 @@ void lcdUpdateFunc(void *unused_param){
 		delay(LCD_RATE);
 	}
 }
+
+void autonomous(){
+	static unsigned long elapsed = 0;
+	opmillis = millis();
+	taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
+	while(1){
+		elapsed = millis() - opmillis;
+		lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));
+
+		if(cannReady){
+			zMotorSet("Misc",127,0);
+			delay(500);
+			zMotorSet("Misc",-127,0);
+			delay(500);
+			zMotorSet("Misc",0,0);
+		}
+		delay(10);
+	}
+}
-- 
cgit v1.2.3