DullesBEST/controller.c at master · DullesRobotics/DullesBEST · GitHub

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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
#pragma config(Sensor, dgtl9,  sonarinput,     sensorSONAR_inch)
#pragma config(Motor,  port1,           light,         tmotorVexFlashlight, openLoop, reversed)
#pragma config(Motor,  port2,           FLM,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           FRM,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           BLM,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           BRM,           tmotorVex393_MC29, openLoop, reversed)
#include "library.h";
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//http://www.robotc.net/wikiarchive/VEX2_Functions_Remote_Control_-_VEXnet#vexRT


int B8RprevVal,B8LprevVal = 0;
int DEFAULT_CONTROL_MODE = 1;
int controlMode = DEFAULT_CONTROL_MODE;
int prevMode = DEFAULT_CONTROL_MODE;
bool flashLightisOn = false;
/***
1 - arcade
2 - tank
3 - Acceleration
4 - Velocity Mode
5 - Autonomous Mode
***/
int NUM_TOTAL_OP_MODES = 4; // I tried #define but it didnt work for some reason
float prevTime;
float dx;
int velx,vely = 0;

//C
task flashControlMode()//run in parallel w/ rest of code
{
	for(int i=1; i<controlMode;i++){
		turnFlashlightOn(light, -127);
		delay(200);
		turnFlashlightOff(light);
		delay(300);
	}
}
void checkBtnAndChangeMode()
{

	if(B8R > B8RprevVal) //If B8R pressed controlMode++
	{
		prevMode = controlMode;
		controlMode = ((controlMode+1)% NUM_TOTAL_OP_MODES)+1;//TODO this may no be working properly
		startTask(flashControlMode);

	};
	if(B8L > B8LprevVal)//If B8L pressed controlMode--
	{
		prevMode = controlMode;
		controlMode = ((controlMode-1)% NUM_TOTAL_OP_MODES)+1;//TODO this may no be working properly
		startTask(flashControlMode);
	}
	B8RprevVal = B8R;
}

task main()
{

	while(true)
	{
		readController();
		checkBtnAndChangeMode();

		if(controlMode == 1)//Arcade Mode
		{
			arcadeDriveLoopContent();

		}
		else if(controlMode == 2)//Tank Mode
		{
			tankDriveLoopContent();
		}
		else if(controlMode == 3)//Accel Mode
		{
			accelDriveLoopContent();
		}

		else if(controlMode == 4)
		{
			if(SensorValue[sonarinput] > 10){
				if(flashLightisOn)
				{
					turnFlashlightOff(light);
					flashLightisOn = false;
				}
				motor[FLM] = 100;
				motor[FRM] = 100;
				motor[BLM] = 100;
				motor[BRM] = 100;
			}
			else{
				if(!flashLightisOn)
				{
					turnFlashlightOn(light, -127);
					flashLightisOn = true;
				}
				motor[FLM] = -100;
				motor[FRM] = -100;
				motor[BLM] = -100;
				motor[BRM] = -100;
			}

		}

	}//end While
}