diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-30 14:30:46 -0400 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-30 14:30:46 -0400 |
| commit | 7afd68829c049cb61e4610836bda8adcf1f7ccee (patch) | |
| tree | 649f9c7ed316ce1722c659c56fcc7810f5e89396 /src/opcontrol.c | |
| parent | f60fcd1bb7b1044a7da5f6ecff89289f091a2773 (diff) | |
first real commit
Diffstat (limited to 'src/opcontrol.c')
| -rw-r--r-- | src/opcontrol.c | 150 |
1 files changed, 73 insertions, 77 deletions
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 <main.h>
+#include <stdint.h>
#include <string.h>
-#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);
}
}
|