hey

13 files changed, 328 insertions, 119 deletions

diff --git a/bin/auto.o b/bin/auto.o
deleted file mode 100644
index eaac073..0000000
--- a/bin/auto.o
+++ /dev/null
diff --git a/bin/init.o b/bin/init.o
deleted file mode 100644
index 46004b7..0000000
--- a/bin/init.o
+++ /dev/null
diff --git a/bin/opcontrol.o b/bin/opcontrol.o
deleted file mode 100644
index 526f94b..0000000
--- a/bin/opcontrol.o
+++ /dev/null
diff --git a/bin/output.bin b/bin/output.bin
deleted file mode 100755
index 3c052d6..0000000
--- a/bin/output.bin
+++ /dev/null
diff --git a/bin/output.elf b/bin/output.elf
deleted file mode 100755
index 72ac408..0000000
--- a/bin/output.elf
+++ /dev/null
diff --git a/bin/robot.o b/bin/robot.o
deleted file mode 100644
index dce8405..0000000
--- a/bin/robot.o
+++ /dev/null
diff --git a/common.mk b/common.mk
index 58098f5..a552764 100644
--- a/common.mk
+++ b/common.mk
@@ -32,7 +32,7 @@ OUTNAME=output.elf
 # Flags for programs

 AFLAGS:=$(MCUAFLAGS)

 ARFLAGS:=$(MCUCFLAGS)

-CCFLAGS:=-c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant

+CCFLAGS:=-s -c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant

 CFLAGS:=$(CCFLAGS) -std=gnu99 -Werror=implicit-function-declaration

 CPPFLAGS:=$(CCFLAGS) -fno-exceptions -fno-rtti -felide-constructors

 LDFLAGS:=-Wall $(MCUCFLAGS) $(MCULFLAGS) -Wl,--gc-sections

diff --git a/include/5106z.h b/include/5106z.h
new file mode 100644
index 0000000..2f2f5c3
--- /dev/null
+++ b/include/5106z.h
@@ -0,0 +1,121 @@
+#ifndef _5106Z_H_
+#define _5106Z_H_
+
+#include <API.h>
+#include <stdint.h>
+#include <stdarg.h>
+
+/*
+ *	motor_init - Initialize motors
+ *
+ *	`count` - The number of motors to enable
+ *	`...`	- The port numbers of the motors to enable
+ *
+ *	Enables motors on the ports specified in the `...` arguments after
+ *	`count`, which should say how many ports are currently being
+ *	initialized. Motors not initialized now will be unusable until they
+ *	are enabled.
+ *
+*/
+void motor_init(uint8_t count,...);
+/*
+ *	motor_initIMEs
+ *
+ *	Sets up IMEs for the motors that have them. This function expects the
+ *	motors passed to it to be in order of how they are plugged in to the
+ *	cortex. There is _no_ error checking, so... yeah.
+ *
+*/
+void motor_initIMEs(uint8_t count,...);
+/*
+ *	motor_imeReset
+ *
+ *	Resets the IME on the motor at port `port` if the motor was init'd
+ *	using motor_initIMEs() and is enabled.
+ *
+*/
+int  motor_imeReset(uint8_t port);
+/*
+ *	motor_imeRead
+ *
+ *	Returns the IME reading of the port `port` if it was init'd using
+ *	motor_initIMEs() and is enabled.
+ *
+*/
+int  motor_imeRead(uint8_t port);
+/*
+ *	motor_enable & motor_disable
+ *
+ *	`port`	- The port of the motor to enable/disable
+ *
+ *	These calls modify the `enable` bit of the motor. If a motor is disabled
+ *	its speed will be set to 0 and rendered unchangeable until the motor is
+ *	re-enabled. Any motor_* calls made to a disabled motor will immediately
+ *	return -1.
+ *
+*/
+void motor_enable(uint8_t port);
+void motor_disable(uint8_t port);
+/*
+ *	motor_togglePolarity
+ *
+ *	`port`	- The port to toggle polarity on
+ *
+ *	Flips the polarity of the motor on port `port` if its enabled. When a
+ *	motor's polarity is enabled the speed written to it is inverted as it
+ *	is passed to motorSet(). For example, if a motor's speed is 127 and it's
+ *	polarity is enabled, the speed passed to motorSet() is -127.
+ *
+*/
+int motor_togglePolarity(uint8_t port);
+/*
+ *	motor_setSpeed
+ *
+ *	`port`	- The port to set the speed of
+ *	`speed`	- The speed to set the motor to
+ *
+ *	Sets the motor to the given speed, if it is enabled. Keep in mind that
+ *	the change is not applied immediately, to apply the speeds you must
+ *	call motor_applySpeeds().
+ *
+*/
+int motor_setSpeed(uint8_t port,int8_t speed);
+/*
+ *	motor_setSpeedSum
+ *
+ *	`port`	- The port to set the speed of
+ *	`args`	- The number of arguments following this one
+ *	`...`	- `args` variables to use for setting the speed
+ *
+ *	Functions the same as motor_setSpeed(), but sets the speed to the sum
+ *	of the arguments passed after `args`.
+ *
+*/
+int motor_setSpeedSum(uint8_t port,uint8_t args,...);
+/*
+ *	TODO
+ *
+*/
+int motor_setSpeedPointer(uint8_t port,int8_t *sp);
+/*
+ *	motor_setThreshold
+ *
+ *	`port`		- The port to set the threshold of
+ *	`threshold`	- The threshold to give the motor
+ *
+ *	Sets a threshold for the motor's speed. This means that when calls to
+ *	set this motor's speed are made in the future, the speed will only be
+ *	applied if it's greater than `threshold` or less than -`threshold`.
+ *	Otherwise, the speed is set to 0.
+ *
+*/
+int motor_setThreshold(uint8_t port,uint8_t threshold);
+/*
+ *	motor_applySpeeds
+ *
+ *	Applys the speeds of the enabled motors to the actual motors.
+ *
+*/
+int motor_applySpeeds(void);
+
+#endif // _5106Z_H_
diff --git a/include/main.h b/include/main.h
index f73b7e3..9b87513 100644
--- a/include/main.h
+++ b/include/main.h
@@ -3,16 +3,17 @@
 

 #include <API.h>

 

+#define LCD_PORT uart2

+

+#define ANALOG_PORT(x) (x+13)

+

 #ifdef __cplusplus

 extern "C" {

 #endif

 

-extern Gyro gyro;

-extern unsigned int imeCount;

-

+void autonomous();

 void initializeIO();

 void initialize();

-void autonomous();

 void operatorControl();

 

 #ifdef __cplusplus

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

 	}

 }