2016/2016CWS1.c at master · team62/2016 · 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
110
111
112
113
114
115
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  piston,         sensorDigitalOut)
#pragma config(Sensor, dgtl2,  brake,          sensorDigitalOut)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           LMbase,        tmotorVex393TurboSpeed_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port2,           LFbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           RBbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           tilt,          tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port5,           RMbase,        tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port6,           RFbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           Rshooter,      tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           Lshooter,      tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           intake,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          LBbase,        tmotorVex393TurboSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	// .....................................................................................
	// Insert user code here.
	// .....................................................................................

	AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	bool held = false;
	bool btn = false;

	while (true)
	{

		btn = (bool) vexRT[Btn8L];
		if(btn && btn != held)
			SensorValue[piston] = !SensorValue[piston];
		held = btn;
		if(!btn) held = false;

		SensorValue[brake] = vexRT[Btn8D];

		motor[RFbase] = -vexRT[Ch2];
		motor[RMbase] = -vexRT[Ch2];
		motor[RBbase] = -vexRT[Ch2];

		motor[LFbase] = vexRT[Ch3];
		motor[LMbase] = vexRT[Ch3];
		motor[LBbase] = vexRT[Ch3];

		if(vexRT[Btn6D])
			motor[tilt] = 127;
		else if(vexRT[Btn6U])
			motor[tilt] = -127;
		else
			motor[tilt] = 0;

		motor[Rshooter] = vexRT[Btn5D]*127;
		motor[Lshooter] = vexRT[Btn5D]*127;

		if(vexRT[Btn5U])
			motor[intake] = 127;
		else if(vexRT[Btn8U])
			motor[intake] = -127;
		else
			motor[intake] = 0;
	}
}