Merge branch 'master' of https://github.com/drumsetmonkey/vex5106z

1 files changed, 125 insertions, 0 deletions

diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
new file mode 100644
index 0000000..66fe496
--- /dev/null
+++ b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
@@ -0,0 +1,125 @@
+/** @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

+ * default priority and stack size whenever the robot is enabled via the Field Management System

+ * or the VEX Competition Switch in the operator control mode. If the robot is disabled or

+ * communications is lost, the operator control task will be stopped by the kernel. Re-enabling

+ * the robot will restart the task, not resume it from where it left off.

+ *

+ * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will

+ * run the operator control task. Be warned that this will also occur if the VEX Cortex is

+ * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.

+ *

+ * Code running in this task can take almost any action, as the VEX Joystick is available and

+ * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly

+ * recommended to give other tasks (including system tasks such as updating LCDs) time to run.

+ *

+ * This task should never exit; it should end with some kind of infinite loop, even if empty.

+ *

+lDist*r=theta

+rDist*(r+15)=theta

+cDist*(r+7.5)=theta

+theta/lDist=r

+rDist*(theta/lDist+15)=theta

+rDist*theta/lDist+15*rDist=theta

+rDist/lDist+15*rDist/theta=1

+1-15*rDist/theta=rDist/lDist

+theta=(1-15*rDist)*lDist/rDist

+theta=(lDist-15*rDist*lDist)/rDist

+theta=lDist/rDist-heading*lDist

+theta=

+*/

+static int 		rRot,lRot,cRot,time1,time2,iHeading;

+static float 	lDist,

+				theta,

+				rDist,

+				cDist,

+				wheelD=3,

+				pi=3.14159,

+				heading=15,

+				startTheta,

+				fHeading;

+/* notes for Clyne:

+ * Other processes should not interupt the collection of data:

+ * rotation calculation should be and data collection should be a high priority/primary task

+ * Main task= manages tasks, init variables, reset functions (sensors, etc)

+ * Primary = sensor data collections and calculations

+ * Secondary = drive tasks

+ *

+ *

+ *

+ */

+void operatorControl() {

+	heading=startTheta;

+	while (1) {

+		time1=micros();

+		cRot=(encoderGet(encoder1)-encoderGet(encoder2));

+		cDist=(cRot*pi)/120;

+		fHeading=(cDist/12)*(360/(2*pi));

+		iHeading=round(fHeading)%360;

+

+		lcdPrint(uart2,1,"iHeading: %d",iHeading);

+

+		if(fHeading<360){

+			motorSet(10,-60);

+			motorSet(1,20);

+

+		}else{

+			motorSet(10,0);

+			motorSet(1,0);

+

+		}

+		/* lRot=encoderGet(encoder1);

+		rRot=encoderGet(encoder2);

+		lDist=(lRot*pi)/120;

+		rDist=(rRot*pi)/120;

+		lcdPrint(uart2,1,"iHeading: %d",iHeading);

+		if(digitalRead(1)==0){

+		motorSet(1,120);

+		}

+		else if(digitalRead(2)==0){

+		motorSet(10,-120);

+		}

+		else{

+		motorSet(1,0);

+		motorSet(10,0);

+		}

+		*/

+		delay(20);

+

+	}

+}