TSA2018/RobotCode_MultipleRoutines.c at master · LRitzdorf/TSA2018 · 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
#pragma config(Motor,  port2,           leftMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           rightMotor,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           liftMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           swivelMotor,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           clawMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Sensor, dgtl10,          redLED,        sensorLEDtoVCC)
#pragma config(Sensor, dgtl11,          yellowLED,     sensorLEDtoVCC)
#pragma config(Sensor, dgtl12,          greenLED,      sensorLEDtoVCC)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX2)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

/*-----------------------------------------------------------------------------------
Robot code for GHS VEX Team G
2018-19 Competiton Season

Team members are Lucas Ritzdorf, Riker Foster, and Lauryn Vornbrock.

In this code, all functions for manually controlled movement are inside
separate tasks.
-----------------------------------------------------------------------------------*/

/*
Remote mappings:
Left stick Y-value -- left drive ------- Ch3
Right stick Y-value - right drive ------ Ch2
Right buttons U, D -- lift up, down ---- Btn8U, Btn8D
Right buttons L, R -- claw swivel ------ Btn8L, Btn8R
Left buttons L, R --- claw open, close - Btn7L, Btn7R
TODO: Check claw motor directions
*/

turnLEDOn(yellowLED);

// Variable Setup

// Standard sleep value (in milliseconds)
// Can be adjusted for responsiveness as needed
const int sleepValue = 20;


void pre_auton() {
	turnLEDOn(redLED);
	bStopTasksBetweenModes = true;
	// Pre-driving setup
	// Set servo positions, etc.
	turnLEDOff(redLED);
	return;
}

task autonomous() {
	turnLEDOn(greenLED);

	// Autonomous code

	turnLEDOff(greenLED);
}

task leftDrive() {
	while (true) {
		motor[leftMotor] = vexRT[Ch3];
		sleep(sleepValue);
	}
}

task rightDrive() {
	while (true) {
		motor[rightMotor] = vexRT[Ch2];
		sleep(sleepValue);
	}
}

task clawLift() {
	while (true) {
		if (vexRT[Btn8U] == 1) { motor[liftMotor] = 127; }
		else if (vexRT[Btn8D] == 1) { motor[liftMotor] = -127; }
}

task clawSwivel() {
	while (true) {
		if (vexRT[Btn8L] == 1) { motor[swivelMotor] = 127; }
		else if (vexRT[Btn8R] == 1) { motor[swivelMotor] = -127; }
}

task clawGrip() {
	while (true) {
		if (vexRT[Btn7L] == 1) { motor[clawMotor] = 127; }
		else if (vexRT[Btn7R] == 1) { motor[clawMotor] = -127; }
}

// Additional movement tasks here

task usercontrol() {
	turnLEDOn(greenLED);
	turnLEDOff(yellowLED);

	startTask(leftDrive);
	startTask(rightDrive);
}