diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-22 14:47:59 -0400 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-22 14:47:59 -0400 |
| commit | f60fcd1bb7b1044a7da5f6ecff89289f091a2773 (patch) | |
| tree | c22a6a5b32b8427d90f07578343e3e42f8d36559 /src/opcontrol.c | |
| parent | c16eeb08e60e724b77bdf173a324175039d4019a (diff) | |
hey
Diffstat (limited to 'src/opcontrol.c')
| -rw-r--r-- | src/opcontrol.c | 142 |
1 files changed, 88 insertions, 54 deletions
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
}
}
|