hey

4 files changed, 201 insertions, 114 deletions

diff --git a/src/5106z.c b/src/5106z.c
new file mode 100644
index 0000000..83462ce
--- /dev/null
+++ b/src/5106z.c
@@ -0,0 +1,105 @@
+#include <5106z.h>
+#include <string.h>
+
+typedef struct {
+	 int8_t *speedP;
+	 int8_t  speed;
+	uint8_t  thresh;
+	uint8_t  polarity	:1;
+	uint8_t  enable		:1;
+	uint8_t  useptr		:1;
+	uint8_t  imeable	:3;
+} __attribute__ ((packed)) motor_t;
+
+static motor_t motor[11];
+
+void motor_init(uint8_t count,...){
+	va_list m;
+	uint8_t i;
+	memset(motor,0,11*sizeof(motor_t));
+	va_start(m,count);
+	for(i=0;i<count;i++){
+		motor[va_arg(m,int)].enable=true;
+	}
+	va_end(m);
+}
+
+void motor_initIMEs(uint8_t count,...){
+	va_list m;
+	uint8_t i;
+	va_start(m,count);
+	for(i=0;i<count;i++){
+		motor[va_arg(m,int)].imeable=i;
+	}
+	va_end(m);
+}
+
+int motor_imeReset(uint8_t port){
+	if(!motor[port].enable) return -1;
+	if(!motor[port].imeable)return -2;
+	imeReset(motor[port].imeable);
+	return 0;
+}
+
+int motor_imeRead(uint8_t port){
+	int imeValue;
+	if(!motor[port].enable) return -1;
+	if(!motor[port].imeable)return -2;
+	if(!imeGet(motor[port].imeable,&imeValue))return -3;
+	return imeValue;
+}
+
+void motor_enable(uint8_t port){
+	motor[port].enable=true;
+}
+
+void motor_disable(uint8_t port){
+	motor[port].enable=false;
+}
+
+int motor_togglePolarity(uint8_t port){
+	if(!motor[port].enable)return -1;
+	return (motor[port].polarity^=true);
+}
+
+int motor_setSpeed(uint8_t port,int8_t speed){
+	if(!motor[port].enable)return -1;
+	return (motor[port].speed=abs(speed)>motor[port].thresh?speed:0);
+}
+
+int motor_setSpeedSum(uint8_t port,uint8_t args,...){
+	va_list s;
+	uint8_t i;
+	int8_t speed=0;
+	if(!motor[port].enable)return -1;
+	va_start(s,args);
+	for(i=0;i<args;i++){
+		speed+=va_arg(s,int);
+	}
+	va_end(s);
+	return motor_setSpeed(port,speed);
+}
+
+int motor_setSpeedPointer(uint8_t port,int8_t *sp){
+	if(!motor[port].enable)return -1;
+	motor[port].useptr=true;
+	motor[port].speedP=sp;
+	return 0;
+}
+
+int motor_setThreshold(uint8_t port,uint8_t threshold){
+	if(!motor[port].enable)return -1;
+	return (motor[port].thresh=threshold);
+}
+
+int motor_applySpeeds(void){
+	uint8_t i;
+	for(i=0;i<10;i++){
+		if(motor[i].enable){
+			if(motor[i].useptr)
+				motor[i].speed=*motor[i].speedP;
+			motorSet(i,motor[i].speed*(motor[i].polarity?-1:1));
+		}
+	}
+	return 0;
+}
diff --git a/src/auto.c b/src/auto.c
index 9df57af..50fda48 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,52 +1,4 @@
-/** @file auto.c

- * @brief File for autonomous code

- *

- * This file should contain the user autonomous() 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>

 

-#include "main.h"

-

-/*

- * Runs the user autonomous 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 autonomous mode. If the robot is disabled or communications is

- * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart

- * the task, not re-start it from where it left off.

- *

- * Code running in the autonomous task cannot access information from the VEX Joystick. However,

- * the autonomous function can be invoked from another task if a VEX Competition Switch is not

- * available, and it can access joystick information if called in this way.

- *

- * The autonomous task may exit, unlike operatorControl() which should never exit. If it does

- * so, the robot will await a switch to another mode or disable/enable cycle.

- */

-void autonomous() {

+void autonomous(){

 }

diff --git a/src/init.c b/src/init.c
index f3948f0..24b5b50 100644
--- a/src/init.c
+++ b/src/init.c
@@ -1,15 +1,11 @@
-#include "main.h"

+#include <main.h>

 

-Gyro gyro;

-unsigned int imeCount=0;

+unsigned int imeCount;

 

-void initializeIO(void){

-	pinMode(1,INPUT_ANALOG);

+void initializeIO(){

+	pinMode(ANALOG_PORT(1),INPUT_ANALOG);

 }

 

-void initialize(void){

-	lcdInit(uart1);

-	lcdClear(uart1);

-	gyro=gyroInit(2,0);

-	imeCount=imeInitializeAll();

+void initialize(){

+	imeCount = imeInitializeAll();

 }

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

 	}

 }