From 0fb3c40c38905790f47cbd77f7e26553bb0061fa Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Wed, 2 Dec 2015 19:43:40 -0500
Subject: RPM/cannon stuff

---
 src/opcontrol.c | 62 +++++++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 54 insertions(+), 8 deletions(-)

(limited to 'src/opcontrol.c')

diff --git a/src/opcontrol.c b/src/opcontrol.c
index e5955eb..9eefe95 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,10 +1,18 @@
 #include <main.h>
+#include <math.h>
+
+const char *song  = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5";
+const char *xpst  = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5";
+const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5";
+const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5.,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\
+4a#5,8g5,8f5,4d#5";
+static unsigned char cyc = 0;
 
 void lcdUpdateFunc(void *);
 
 void operatorControl(){
 
-	static char lift;
+	static char lift,cann;
 
 	zLCDStart();
 	zLCDSetUpdateFunc(lcdUpdateFunc);
@@ -19,27 +27,65 @@ void operatorControl(){
 
 		//	Set the rotating motor speed.
 
-		zMotorSet("ROTATER",-zJoyAnalog(2,1)/4);
+		zMotorSet("Rotater",-zJoyAnalog(2,1)/4);
 
 		// Set the intake's speed.
 
-		zMotorSet("INTAKE",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));
+		zMotorSet("Intake",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));
 
 		// Set the lift's speed.
 
 		lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127);
 
-		zMotorSet("LIFT1",lift);
-		zMotorSet("LIFT2",lift);
+		zMotorSet("Lift 1",lift);
+		zMotorSet("Lift 2",lift);
+
+		cann=zJoyAnalog(2,3);//zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127);
+
+		zMotorSet("Left cannon" ,-cann);
+		zMotorSet("Right cannon", cann);
+
+		zMotorSet("Misc",zGetDigitalMotorSpeed(2,7,JOY_UP,JOY_DOWN,127));
+
+		if(zJoyDigital(1,7,JOY_LEFT)){
+			speakerInit();
+			switch(cyc){
+			case 0:speakerPlayRtttl(song );break;
+			case 1:speakerPlayRtttl(xpst );break;
+			case 2:speakerPlayRtttl(xpst2);break;
+			case 3:speakerPlayRtttl(bound);break;
+			}
+			if(++cyc == 4) cyc = 0;
+			speakerShutdown();
+		}
 
 		delay(10);	// Short delay to allow task switching
 	}
 }
 
 void lcdUpdateFunc(void *unused_param){
-	static double liftIME;
+	static double l,r,dist,heading;
+	static double cl,cr,ca;
+
+	/*
+	 *	Positioning code.
+	*/
+
+	l =  zMotorIMEGet("Left drive")  / 627.2L;
+	r = -zMotorIMEGet("Right drive") / 627.2L;
+
+	dist=(l - r) * 8.64L;
+	heading = fmod(round(dist / 15),360.0L);
+
+	zLCDWrite(1,"%.3lf %.3lf",heading,dist);
+	//zLCDWrite(2,"%.3lf %.3lf",l,r);
 
-	liftIME = (zMotorIMEGetVelocity("LIFT1") - zMotorIMEGetVelocity("LIFT2")) / 2 / 16.3333125L;
+	/*
+	 *	RPM control code.
+	*/
 
-	zLCDWrite(1,"%.3lf",liftIME);
+	cl = -zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;
+	cr =  zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;
+	ca = (cl + cr) / 2;
+	zLCDWrite(2,"RPM: %.3lf",ca);
 }
-- 
cgit v1.2.3