+++ /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_
+++ /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;
-}
#include <main.h>\r
+#include <stdint.h>\r
#include <string.h>\r
-#include <5106z.h>\r
\r
enum MOTOR_PORT_MAP {\r
UNUSED = 0,\r
- ROTATER,\r
- DRIVEL,\r
- DRIVER,\r
- LIFT1,\r
- LIFT2,\r
INTAKE1,\r
+ DRIVEL ,\r
+ DRIVER ,\r
+ LIFT1 ,\r
+ LIFT2 ,\r
+ UNUSED6,\r
INTAKE2,\r
INTAKE3,\r
INTAKE4,\r
- PULLER,\r
+ ROTATER,\r
};\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
+extern Gyro gyro;\r
+\r
+static signed int ctty=0;\r
+const unsigned int mtty=3;\r
\r
-void shell(void *unused){\r
- char *input=(char *)malloc(4*sizeof(char));\r
+void operatorLCD(void *unused){\r
while(1){\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
+ switch(ctty){\r
+ default:\r
+ case 0:\r
+ lcdSetText(uart1,1," JOHN CENA ");\r
+ lcdSetText(uart1,2," ============ ");\r
break;\r
- case 't':\r
- printf("Test\n\r");\r
+ case 1:\r
+ lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f));\r
+ lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1) / 210.0f));\r
break;\r
+ case 2:\r
+ lcdPrint(uart1,1,"Gyro: %u",gyroGet(gyro));\r
}\r
+ delay(2000);\r
}\r
}\r
\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
+#define getJoy(n) joy[n-1]\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
+ static int8_t joy[8],\r
+ lift = 0,\r
+ rotate = 0,\r
+ intake = 0,\r
+ uiinc = 0;\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
+ // Start the LCD task\r
+ taskCreate(operatorLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
\r
while(1){\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
+ joy[0]=joystickGetAnalog(1,1);\r
+ joy[1]=joystickGetAnalog(1,2);\r
+ joy[2]=joystickGetAnalog(1,3);\r
+ joy[3]=joystickGetAnalog(1,4);\r
+ joy[4]=joystickGetAnalog(2,1);\r
+ joy[5]=joystickGetAnalog(2,2);\r
+ joy[6]=joystickGetAnalog(2,3);\r
+ joy[7]=joystickGetAnalog(2,4);\r
+\r
+ intake = getJoy(7);\r
+ rotate =-getJoy(5);\r
+ lift = joystickGetDigital(2,6,JOY_UP ) ? 127 :\r
+ joystickGetDigital(2,6,JOY_DOWN) ? -127 :\r
+ 0;\r
+\r
+ motorSet(INTAKE1,intake);\r
+ motorSet(INTAKE2,intake);\r
+ motorSet(INTAKE3,intake);\r
+ motorSet(INTAKE4,intake);\r
+ motorSet(ROTATER,rotate);\r
+ motorSet(LIFT1,lift);\r
+ motorSet(LIFT2,lift);\r
+\r
+ motorSet(DRIVEL,-getJoy(2));\r
+ motorSet(DRIVER, getJoy(3));\r
+\r
+ /*motorSet(PULLER,joystickGetDigital(2,6,JOY_UP ) ? 127 :\r
+ joystickGetDigital(2,6,JOY_DOWN) ? -127 :\r
+ 0);*/\r
+\r
+ if(++uiinc==20){\r
+ uiinc=0;\r
+ if(joystickGetDigital(1,7,JOY_UP) ||\r
+ joystickGetDigital(2,7,JOY_UP) ){\r
+ if(++ctty==mtty)ctty=0;\r
+ }\r
+ else if(joystickGetDigital(1,7,JOY_DOWN) ||\r
+ joystickGetDigital(2,7,JOY_DOWN) ){\r
+ if(--ctty==-1)ctty=mtty-1;\r
+ }\r
+ }\r
\r
- motor_applySpeeds(); // Apply the motor speeds\r
+ delay(10);\r
}\r
}\r