1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
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);
}
}
|
