RoboticsCompetition/Movement.c at master · WilliamPlusPlus/RoboticsCompetition · 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
#pragma config(Motor,  port2,           largeLeft,     tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           largeRight,    tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port4,           Forklift,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           launchServo,   tmotorServoStandard, openLoop)


task main()
{


int direction = 0;
//1 = Forward && 2 = Backward && 0 = Nothing


while (true) {


		if((vexRT[Btn8L] == 1)) {
			motor[launchServo] = 127;
			//motor[launchServo] = 127;
			} else {
			motor[launchServo] = -127;
			//motor[launchServo] = -127;
		}

		if (abs(vexRT[Ch2]) > 0.5) {
			motor[Forklift] = (vexRT[Ch2]);
		}


		// Handle Driving on Left Joystick
		if (abs(vexRT[Ch3]) > abs(vexRT[Ch4])) {
			//Forward Drive
			motor[largeLeft] = vexRT[Ch3];
			motor[largeRight] = vexRT[Ch3];
			} else {
			if (vexRT[Ch3] >= -50) {
			if (abs (vexRT[Ch4]) > 25) {
				// Wide Turn Right
				motor[largeLeft] = vexRT[Ch4];
				motor[largeRight] = vexRT[Ch4] * 0.2;
			}

			if ((vexRT[Ch4]) < -25) {
				// Wide Turn Left
				motor[largeRight] = vexRT[Ch4] * -1;
				motor[largeLeft] = vexRT[Ch4] * -0.2;
			}
		} else { // This is if channel 4 (left joystick y) is negative
			if (abs (vexRT[Ch4]) > 25) {
				// Wide Turn Right
				motor[largeLeft] = -(vexRT[Ch4]);
				motor[largeRight] = -(vexRT[Ch4] * 0.2);
			}

			if ((vexRT[Ch4]) < -25) {
				// Wide Turn Left
				motor[largeRight] = -(vexRT[Ch4] * -1);
				motor[largeLeft] = -(vexRT[Ch4] * -0.2);
			}
	}
}
	}
}