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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
|
#include "main.h"
#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*
lDist*r=theta
rDist*(r+15)=theta
cDist*(r+7.5)=theta
theta/lDist=r
rDist*(theta/lDist+15)=theta
rDist*theta/lDist+15*rDist=theta
rDist/lDist+15*rDist/theta=1
1-15*rDist/theta=rDist/lDist
theta=(1-15*rDist)*lDist/rDist
theta=(lDist-15*rDist*lDist)/rDist
theta=lDist/rDist-heading*lDist
theta=
*/
static int rRot,lRot,cRot,time1,time2,iHeading;
static float lDist,
theta,
rDist,
cDist,
wheelD=3,
pi=3.14159,
heading=15,
startTheta,
fHeading;
/* notes for Clyne:
* Other processes should not interupt the collection of data:
* rotation calculation should be and data collection should be a high priority/primary task
* Main task= manages tasks, init variables, reset functions (sensors, etc)
* Primary = sensor data collections and calculations
* Secondary = drive tasks
*
*
*
*/
void operatorControl() {
heading=startTheta;
while (1) {
time1=micros();
cRot=(encoderGet(encoder1)-encoderGet(encoder2));
cDist=(cRot*pi)/120;
fHeading=(cDist/12)*(360/(2*pi));
iHeading=round(fHeading)%360;
lcdPrint(uart2,1,"iHeading: %d",iHeading);
if(fHeading<360){
motorSet(10,-60);
motorSet(1,20);
}else{
motorSet(10,0);
motorSet(1,0);
}
/* lRot=encoderGet(encoder1);
rRot=encoderGet(encoder2);
lDist=(lRot*pi)/120;
rDist=(rRot*pi)/120;
lcdPrint(uart2,1,"iHeading: %d",iHeading);
if(digitalRead(1)==0){
motorSet(1,120);
}
else if(digitalRead(2)==0){
motorSet(10,-120);
}
else{
motorSet(1,0);
motorSet(10,0);
}
*/
delay(20);
}
}
|
