diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-06 14:41:38 -0400 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2015-10-06 14:41:38 -0400 |
| commit | 51699171c289c5ff553ed0c9c75c5a108e164fff (patch) | |
| tree | db9379fd4b8966bc12bc2713912638c691b3db1b /src/opcontrol.c | |
| parent | e5199012ee77fd8ada1bffdb1a44057ef98a25a8 (diff) | |
uploaded code
Diffstat (limited to 'src/opcontrol.c')
| -rw-r--r-- | src/opcontrol.c | 72 |
1 files changed, 72 insertions, 0 deletions
diff --git a/src/opcontrol.c b/src/opcontrol.c new file mode 100644 index 0000000..b2896a2 --- /dev/null +++ b/src/opcontrol.c @@ -0,0 +1,72 @@ +#include "main.h"
+
+#define NEAR_THRESH 0.05f
+
+enum MOTOR {
+ UNUSED = 0,
+ ROTATER,
+ DRIVEL,
+ DRIVER,
+ LIFT1,
+ LIFT2,
+ INTAKE,
+ UNUSED7,
+ UNUSED8,
+ UNUSED9,
+ UNUSED10
+};
+
+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;
+}
+
+void operatorControlLCD(void *param){
+ static int imeValue=0;
+ static float nearTarget=0;
+ 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++;
+ }
+ 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);
+ 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);
+ }
+}
|