From f60fcd1bb7b1044a7da5f6ecff89289f091a2773 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Thu, 22 Oct 2015 14:47:59 -0400
Subject: hey

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

(limited to 'src/opcontrol.c')

diff --git a/src/opcontrol.c b/src/opcontrol.c
index b2896a2..6a2adee 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,72 +1,106 @@
-#include "main.h"
+#include <main.h>
+#include <string.h>
+#include <5106z.h>
 
-#define NEAR_THRESH 0.05f
-
-enum MOTOR {
+enum MOTOR_PORT_MAP {
 	UNUSED = 0,
 	ROTATER,
 	DRIVEL,
 	DRIVER,
 	LIFT1,
 	LIFT2,
-	INTAKE,
-	UNUSED7,
-	UNUSED8,
-	UNUSED9,
-	UNUSED10
+	INTAKE1,
+	INTAKE2,
+	INTAKE3,
+	INTAKE4,
+	PULLER,
 };
 
-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;
-}
+static int8_t joyx   = 0,	// Contains the left joystick's x position
+			  joyy   = 0,	// Contains the left joystick's y position
+			  lift   = 0,	// Contains the desired speed for the lift
+			  rotate = 0;	// Contains the desired speed for the lift's axis
 
-void operatorControlLCD(void *param){
-	static int imeValue=0;
-	static float nearTarget=0;
+void shell(void *unused){
+	char *input=(char *)malloc(4*sizeof(char));
 	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++;
+		printf("debug@5106Z > ");
+		memset(input,0,4);
+		//fgets(input,4,stdin);
+		input[0]=fgetc(stdin);
+		printf("\n\r");
+		switch(input[0]){
+		case 'v':
+			input[2]=fgetc(stdin);
+			printf("\n\r");
+			if(input[2]=='m')
+				printf("Main voltage: %1.3f V\n\r",powerLevelMain()/1000.0f);
+			else if(input[2]=='e')
+				printf("Expander voltage: %1.3f V\n\r",analogRead(1)/70.0f);
+			break;
+		case 't':
+			printf("Test\n\r");
+			break;
 		}
-		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);
+void operatorControl(){
+
+	motor_init(10,			// Initialize 6 motor ports
+			   ROTATER,
+			   DRIVEL,
+			   DRIVER,
+			   LIFT1,
+			   LIFT2,
+			   INTAKE1,
+			   INTAKE2,
+			   INTAKE3,
+			   INTAKE4,
+			   PULLER);
+
+	motor_togglePolarity(DRIVER );	// Flip the right drive motors
+	motor_togglePolarity(ROTATER);	// Flip the lift's rotation motor
+
+
+
+	motor_setSpeedPointer(LIFT1  ,&lift  );	// Always set the lift speed with `lift`
+	motor_setSpeedPointer(LIFT2  ,&lift  );	//
+	motor_setSpeedPointer(ROTATER,&rotate);	// Always set the lift's axis speed
+											// with `rotate`
+
+	extern unsigned int imeCount;
+	motor_initIMEs(imeCount,
+				   DRIVER,
+				   DRIVEL,
+				   0,
+				   0,
+				   ROTATER);
+
+	// Launch the shell
+	taskCreate(shell,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);
+
+		digitalWrite(1,lcdReadButtons(LCD_PORT));
+
+		joyx   = joystickGetAnalog(1,4);	// Get joystick positions
+		joyy   = joystickGetAnalog(1,3);	//
+		lift   = joystickGetAnalog(1,2);	//
+		rotate = joystickGetAnalog(1,1);	//
+
+		motor_setSpeedSum(DRIVEL,2,joyy, joyx);	// Set drive speeds
+		motor_setSpeedSum(DRIVER,2,joyy,-joyx);	//
+
+		static char huh;
+		huh=joystickGetDigital(1,8,JOY_UP)?127:joystickGetDigital(1,8,JOY_DOWN)?-127:0;
+		motor_setSpeed(INTAKE1,huh);
+		motor_setSpeed(INTAKE2,huh);
+		motor_setSpeed(INTAKE3,huh);
+		motor_setSpeed(INTAKE4,huh);
+
+		motor_setSpeed(PULLER,joystickGetDigital(1,7,JOY_UP)?127:joystickGetDigital(1,7,JOY_DOWN)?-127:0);
+
+		motor_applySpeeds();	// Apply the motor speeds
 	}
 }
-- 
cgit v1.2.3