RPM/cannon stuff

4 files changed, 77 insertions, 59 deletions

diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
index 66fe496..3ac3d9b 100644
--- a/Nate's Position Testing/Shooter Testing/src/opcontrol.c
+++ b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
@@ -1,37 +1,5 @@
-/** @file opcontrol.c

- * @brief File for operator control code

- *

- * This file should contain the user operatorControl() function and any functions related to it.

- *

- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.

- * All rights reserved.

- *

- * Redistribution and use in source and binary forms, with or without

- * modification, are permitted provided that the following conditions are met:

- *     * Redistributions of source code must retain the above copyright

- *       notice, this list of conditions and the following disclaimer.

- *     * Redistributions in binary form must reproduce the above copyright

- *       notice, this list of conditions and the following disclaimer in the

- *       documentation and/or other materials provided with the distribution.

- *     * Neither the name of Purdue University ACM SIG BOTS nor the

- *       names of its contributors may be used to endorse or promote products

- *       derived from this software without specific prior written permission.

- *

- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND

- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY

- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

- *

- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be

- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.

- */

 #include "main.h"

+

 #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))

 /*

  * Runs the user operator control code. This function will be started in its own task with the

diff --git a/include/zephyr.h b/include/zephyr.h
index 844416a..1ba234c 100644
--- a/include/zephyr.h
+++ b/include/zephyr.h
@@ -3,6 +3,8 @@
 
 #include <main.h>
 
+#define PI	3.14159265L
+
 #define APPLY_THRESH(n,t)	if(n < t && n > -t){ n = 0;}
 
 /*
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);

 }

diff --git a/src/zephyr.c b/src/zephyr.c
index 847f95a..7bfd76e 100644
--- a/src/zephyr.c
+++ b/src/zephyr.c
@@ -81,30 +81,32 @@ void zGyroInit(void){
 #define MOTOR_PORT_COUNT	10
 
 #ifdef IME_ENABLE
-#define MOTOR_IME_COUNT		5
+#define MOTOR_IME_COUNT		7
 #endif // IME_ENABLE
 
 const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
-	"UNUSED1",
-	"UNUSED2",
-	"UNUSED3",
-	"UNUSED4",
-	"INTAKE",
-	"DRIVER",
-	"DRIVEL",
-	"LIFT1",
-	"LIFT2",
-	"ROTATER"
+	"Left cannon",
+	"Right cannon",
+	"Misc",
+	"Port 4",
+	"Intake",
+	"Right drive",
+	"Left drive",
+	"Lift 1",
+	"Lift 2",
+	"Rotater"
 };
 
 #ifdef IME_ENABLE
 
 const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = {
-	"DRIVER",
-	"DRIVEL",
-	"ROTATER",
-	"LIFT1",
-	"LIFT2"
+	"Right drive",
+	"Left drive",
+	"Rotater",
+	"Lift 1",
+	"Lift 2",
+	"Left cannon",
+	"Right cannon"
 };
 
 static unsigned int imeCount = 0;
@@ -179,8 +181,8 @@ void zDriveUpdate(void){
 	APPLY_THRESH(l,DRIVE_THRESHOLD);
 	APPLY_THRESH(r,DRIVE_THRESHOLD);
 
-	zMotorSet("DRIVEL",l);
-	zMotorSet("DRIVER",r);
+	zMotorSet("Left drive",l);
+	zMotorSet("Right drive",r);
 
 #endif // DRIVE_NORMAL