From 7afd68829c049cb61e4610836bda8adcf1f7ccee Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Fri, 30 Oct 2015 14:30:46 -0400 Subject: first real commit --- src/opcontrol.c | 150 +++++++++++++++++++++++++++----------------------------- 1 file changed, 73 insertions(+), 77 deletions(-) (limited to 'src/opcontrol.c') diff --git a/src/opcontrol.c b/src/opcontrol.c index 6a2adee..bd3118a 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -1,106 +1,102 @@ #include +#include #include -#include <5106z.h> enum MOTOR_PORT_MAP { UNUSED = 0, - ROTATER, - DRIVEL, - DRIVER, - LIFT1, - LIFT2, INTAKE1, + DRIVEL , + DRIVER , + LIFT1 , + LIFT2 , + UNUSED6, INTAKE2, INTAKE3, INTAKE4, - PULLER, + ROTATER, }; -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 +extern Gyro gyro; + +static signed int ctty=0; +const unsigned int mtty=3; -void shell(void *unused){ - char *input=(char *)malloc(4*sizeof(char)); +void operatorLCD(void *unused){ while(1){ - 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); + switch(ctty){ + default: + case 0: + lcdSetText(uart1,1," JOHN CENA "); + lcdSetText(uart1,2," ============ "); break; - case 't': - printf("Test\n\r"); + case 1: + lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f)); + lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1) / 210.0f)); break; + case 2: + lcdPrint(uart1,1,"Gyro: %u",gyroGet(gyro)); } + delay(2000); } } 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 - - +#define getJoy(n) joy[n-1] - 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` + static int8_t joy[8], + lift = 0, + rotate = 0, + intake = 0, + uiinc = 0; - 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); + // Start the LCD task + taskCreate(operatorLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); while(1){ - 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); + joy[0]=joystickGetAnalog(1,1); + joy[1]=joystickGetAnalog(1,2); + joy[2]=joystickGetAnalog(1,3); + joy[3]=joystickGetAnalog(1,4); + joy[4]=joystickGetAnalog(2,1); + joy[5]=joystickGetAnalog(2,2); + joy[6]=joystickGetAnalog(2,3); + joy[7]=joystickGetAnalog(2,4); + + intake = getJoy(7); + rotate =-getJoy(5); + lift = joystickGetDigital(2,6,JOY_UP ) ? 127 : + joystickGetDigital(2,6,JOY_DOWN) ? -127 : + 0; + + motorSet(INTAKE1,intake); + motorSet(INTAKE2,intake); + motorSet(INTAKE3,intake); + motorSet(INTAKE4,intake); + motorSet(ROTATER,rotate); + motorSet(LIFT1,lift); + motorSet(LIFT2,lift); + + motorSet(DRIVEL,-getJoy(2)); + motorSet(DRIVER, getJoy(3)); + + /*motorSet(PULLER,joystickGetDigital(2,6,JOY_UP ) ? 127 : + joystickGetDigital(2,6,JOY_DOWN) ? -127 : + 0);*/ + + if(++uiinc==20){ + uiinc=0; + if(joystickGetDigital(1,7,JOY_UP) || + joystickGetDigital(2,7,JOY_UP) ){ + if(++ctty==mtty)ctty=0; + } + else if(joystickGetDigital(1,7,JOY_DOWN) || + joystickGetDigital(2,7,JOY_DOWN) ){ + if(--ctty==-1)ctty=mtty-1; + } + } - motor_applySpeeds(); // Apply the motor speeds + delay(10); } } -- cgit v1.2.3