# Flags for programs\r
AFLAGS:=$(MCUAFLAGS)\r
ARFLAGS:=$(MCUCFLAGS)\r
-CCFLAGS:=-c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant\r
+CCFLAGS:=-s -c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant\r
CFLAGS:=$(CCFLAGS) -std=gnu99 -Werror=implicit-function-declaration\r
CPPFLAGS:=$(CCFLAGS) -fno-exceptions -fno-rtti -felide-constructors\r
LDFLAGS:=-Wall $(MCUCFLAGS) $(MCULFLAGS) -Wl,--gc-sections\r
--- /dev/null
+#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_
\r
#include <API.h>\r
\r
+#define LCD_PORT uart2\r
+\r
+#define ANALOG_PORT(x) (x+13)\r
+\r
#ifdef __cplusplus\r
extern "C" {\r
#endif\r
\r
-extern Gyro gyro;\r
-extern unsigned int imeCount;\r
-\r
+void autonomous();\r
void initializeIO();\r
void initialize();\r
-void autonomous();\r
void operatorControl();\r
\r
#ifdef __cplusplus\r
--- /dev/null
+#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;
+}
-/** @file auto.c\r
- * @brief File for autonomous code\r
- *\r
- * This file should contain the user autonomous() function and any functions related to it.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- * * Redistributions of source code must retain the above copyright\r
- * notice, this list of conditions and the following disclaimer.\r
- * * Redistributions in binary form must reproduce the above copyright\r
- * notice, this list of conditions and the following disclaimer in the\r
- * documentation and/or other materials provided with the distribution.\r
- * * Neither the name of Purdue University ACM SIG BOTS nor the\r
- * names of its contributors may be used to endorse or promote products\r
- * derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
+#include <main.h>\r
\r
-#include "main.h"\r
-\r
-/*\r
- * Runs the user autonomous code. This function will be started in its own task with the default\r
- * priority and stack size whenever the robot is enabled via the Field Management System or the\r
- * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is\r
- * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart\r
- * the task, not re-start it from where it left off.\r
- *\r
- * Code running in the autonomous task cannot access information from the VEX Joystick. However,\r
- * the autonomous function can be invoked from another task if a VEX Competition Switch is not\r
- * available, and it can access joystick information if called in this way.\r
- *\r
- * The autonomous task may exit, unlike operatorControl() which should never exit. If it does\r
- * so, the robot will await a switch to another mode or disable/enable cycle.\r
- */\r
-void autonomous() {\r
+void autonomous(){\r
}\r
-#include "main.h"\r
+#include <main.h>\r
\r
-Gyro gyro;\r
-unsigned int imeCount=0;\r
+unsigned int imeCount;\r
\r
-void initializeIO(void){\r
- pinMode(1,INPUT_ANALOG);\r
+void initializeIO(){\r
+ pinMode(ANALOG_PORT(1),INPUT_ANALOG);\r
}\r
\r
-void initialize(void){\r
- lcdInit(uart1);\r
- lcdClear(uart1);\r
- gyro=gyroInit(2,0);\r
- imeCount=imeInitializeAll();\r
+void initialize(){\r
+ imeCount = imeInitializeAll();\r
}\r
-#include "main.h"\r
+#include <main.h>\r
+#include <string.h>\r
+#include <5106z.h>\r
\r
-#define NEAR_THRESH 0.05f\r
-\r
-enum MOTOR {\r
+enum MOTOR_PORT_MAP {\r
UNUSED = 0,\r
ROTATER,\r
DRIVEL,\r
DRIVER,\r
LIFT1,\r
LIFT2,\r
- INTAKE,\r
- UNUSED7,\r
- UNUSED8,\r
- UNUSED9,\r
- UNUSED10\r
+ INTAKE1,\r
+ INTAKE2,\r
+ INTAKE3,\r
+ INTAKE4,\r
+ PULLER,\r
};\r
\r
-static unsigned char idx=0;\r
-static float intakeRotation=0;\r
-\r
-int nearDegree(float target){\r
- if(intakeRotation<target-NEAR_THRESH)return 1;\r
- if(intakeRotation>target+NEAR_THRESH)return -1;\r
- else return 0;\r
-}\r
+static int8_t joyx = 0, // Contains the left joystick's x position\r
+ joyy = 0, // Contains the left joystick's y position\r
+ lift = 0, // Contains the desired speed for the lift\r
+ rotate = 0; // Contains the desired speed for the lift's axis\r
\r
-void operatorControlLCD(void *param){\r
- static int imeValue=0;\r
- static float nearTarget=0;\r
+void shell(void *unused){\r
+ char *input=(char *)malloc(4*sizeof(char));\r
while(1){\r
- imeGet(idx,&imeValue);\r
- intakeRotation=imeValue/1037.0f;\r
- lcdPrint(uart1,1,"%u %d",imeCount,nearDegree(nearTarget));\r
- lcdPrint(uart1,2,"%u %d",idx,imeValue);\r
- if(joystickGetDigital(1,7,JOY_LEFT)){\r
- if(idx)idx--;\r
- }else if(joystickGetDigital(1,7,JOY_RIGHT)){\r
- if(idx<imeCount-1)idx++;\r
+ printf("debug@5106Z > ");\r
+ memset(input,0,4);\r
+ //fgets(input,4,stdin);\r
+ input[0]=fgetc(stdin);\r
+ printf("\n\r");\r
+ switch(input[0]){\r
+ case 'v':\r
+ input[2]=fgetc(stdin);\r
+ printf("\n\r");\r
+ if(input[2]=='m')\r
+ printf("Main voltage: %1.3f V\n\r",powerLevelMain()/1000.0f);\r
+ else if(input[2]=='e')\r
+ printf("Expander voltage: %1.3f V\n\r",analogRead(1)/70.0f);\r
+ break;\r
+ case 't':\r
+ printf("Test\n\r");\r
+ break;\r
}\r
- if(joystickGetDigital(1,8,JOY_LEFT)){\r
- nearTarget-=.25;\r
- }else if(joystickGetDigital(1,8,JOY_RIGHT)){\r
- nearTarget+=.25;\r
- }\r
- delay(300);\r
}\r
}\r
\r
-void operatorControl(void){\r
- static char liftSpeed=0;\r
- static char intakeSpeed=0;\r
- taskCreate(operatorControlLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+void operatorControl(){\r
+\r
+ motor_init(10, // Initialize 6 motor ports\r
+ ROTATER,\r
+ DRIVEL,\r
+ DRIVER,\r
+ LIFT1,\r
+ LIFT2,\r
+ INTAKE1,\r
+ INTAKE2,\r
+ INTAKE3,\r
+ INTAKE4,\r
+ PULLER);\r
+\r
+ motor_togglePolarity(DRIVER ); // Flip the right drive motors\r
+ motor_togglePolarity(ROTATER); // Flip the lift's rotation motor\r
+\r
+\r
+\r
+ motor_setSpeedPointer(LIFT1 ,&lift ); // Always set the lift speed with `lift`\r
+ motor_setSpeedPointer(LIFT2 ,&lift ); //\r
+ motor_setSpeedPointer(ROTATER,&rotate); // Always set the lift's axis speed\r
+ // with `rotate`\r
+\r
+ extern unsigned int imeCount;\r
+ motor_initIMEs(imeCount,\r
+ DRIVER,\r
+ DRIVEL,\r
+ 0,\r
+ 0,\r
+ ROTATER);\r
+\r
+ // Launch the shell\r
+ taskCreate(shell,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+\r
while(1){\r
- // Set drive motors\r
- motorSet(DRIVEL, joystickGetAnalog(1,3));\r
- motorSet(DRIVER,-joystickGetAnalog(1,2));\r
- liftSpeed=-(joystickGetDigital(1,6,JOY_UP )? 127:\r
- joystickGetDigital(1,6,JOY_DOWN)?-127:0);\r
- motorSet(LIFT1,liftSpeed);\r
- motorSet(LIFT2,liftSpeed);\r
- intakeSpeed=-(joystickGetDigital(1,5,JOY_UP )? 127:\r
- joystickGetDigital(1,5,JOY_DOWN)?-127:0);\r
- motorSet(INTAKE,intakeSpeed);\r
- motorSet(ROTATER,joystickGetAnalog(1,1));\r
-\r
- // test motor\r
- //motorSet(10,joystickGetDigital(1,7,JOY_UP)?127:0);\r
-\r
- delay(20);\r
+\r
+ digitalWrite(1,lcdReadButtons(LCD_PORT));\r
+\r
+ joyx = joystickGetAnalog(1,4); // Get joystick positions\r
+ joyy = joystickGetAnalog(1,3); //\r
+ lift = joystickGetAnalog(1,2); //\r
+ rotate = joystickGetAnalog(1,1); //\r
+\r
+ motor_setSpeedSum(DRIVEL,2,joyy, joyx); // Set drive speeds\r
+ motor_setSpeedSum(DRIVER,2,joyy,-joyx); //\r
+\r
+ static char huh;\r
+ huh=joystickGetDigital(1,8,JOY_UP)?127:joystickGetDigital(1,8,JOY_DOWN)?-127:0;\r
+ motor_setSpeed(INTAKE1,huh);\r
+ motor_setSpeed(INTAKE2,huh);\r
+ motor_setSpeed(INTAKE3,huh);\r
+ motor_setSpeed(INTAKE4,huh);\r
+\r
+ motor_setSpeed(PULLER,joystickGetDigital(1,7,JOY_UP)?127:joystickGetDigital(1,7,JOY_DOWN)?-127:0);\r
+\r
+ motor_applySpeeds(); // Apply the motor speeds\r
}\r
}\r
projects / abelleisle / vex5106z.git / commitdiff