1 files changed, 72 insertions, 0 deletions
diff --git a/src/opcontrol.c b/src/opcontrol.c
new file mode 100644
index 0000000..b2896a2
--- /dev/null
+++ b/src/opcontrol.c
@@ -0,0 +1,72 @@
+#include "main.h"
+
+#define NEAR_THRESH 0.05f
+
+enum MOTOR {
+	UNUSED = 0,
+	ROTATER,
+	DRIVEL,
+	DRIVER,
+	LIFT1,
+	LIFT2,
+	INTAKE,
+	UNUSED7,
+	UNUSED8,
+	UNUSED9,
+	UNUSED10
+};
+
+static unsigned char idx=0;
+static float intakeRotation=0;
+
+int nearDegree(float target){
+	if(intakeRotation<target-NEAR_THRESH)return  1;
+	if(intakeRotation>target+NEAR_THRESH)return -1;
+	else				    			 return  0;
+}
+
+void operatorControlLCD(void *param){
+	static int imeValue=0;
+	static float nearTarget=0;
+	while(1){
+		imeGet(idx,&imeValue);
+		intakeRotation=imeValue/1037.0f;
+		lcdPrint(uart1,1,"%u %d",imeCount,nearDegree(nearTarget));
+		lcdPrint(uart1,2,"%u %d",idx,imeValue);
+		if(joystickGetDigital(1,7,JOY_LEFT)){
+			if(idx)idx--;
+		}else if(joystickGetDigital(1,7,JOY_RIGHT)){
+			if(idx<imeCount-1)idx++;
+		}
+		if(joystickGetDigital(1,8,JOY_LEFT)){
+			nearTarget-=.25;
+		}else if(joystickGetDigital(1,8,JOY_RIGHT)){
+			nearTarget+=.25;
+		}
+		delay(300);
+	}
+}
+
+void operatorControl(void){
+	static char liftSpeed=0;
+	static char intakeSpeed=0;
+	taskCreate(operatorControlLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
+	while(1){
+		// Set drive motors
+		motorSet(DRIVEL, joystickGetAnalog(1,3));
+		motorSet(DRIVER,-joystickGetAnalog(1,2));
+		liftSpeed=-(joystickGetDigital(1,6,JOY_UP  )? 127:
+				    joystickGetDigital(1,6,JOY_DOWN)?-127:0);
+		motorSet(LIFT1,liftSpeed);
+		motorSet(LIFT2,liftSpeed);
+		intakeSpeed=-(joystickGetDigital(1,5,JOY_UP  )? 127:
+					  joystickGetDigital(1,5,JOY_DOWN)?-127:0);
+		motorSet(INTAKE,intakeSpeed);
+		motorSet(ROTATER,joystickGetAnalog(1,1));
+
+		// test motor
+		//motorSet(10,joystickGetDigital(1,7,JOY_UP)?127:0);
+
+		delay(20);
+	}
+}