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
|
#include "main.h"
#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#define PI =3.14159
/*
* 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.
*
*/
/* 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
*
*
*
*/
static long lVel,rVel,deltaPos,deltaTime,startTheta,
heading,lDist,rDist,xPos,yPos,
track=15;
void operatorControl() {
heading=startTheta;
while (1) {
//get time 1 the first time
heading=((lDist-rDist)/(track*2*PI))*360+startTheta;//heading in degrees. might need to be radians for cosine functions
//get time 2
deltaPos=(lVel+rVel)/2*deltaTime;
xPos+=cos(heading)*deltaPos;
yPos+=sin(heading)*deltaPos;
//get time 1
delay(20);
}
}
|
