2016/2016CWS4.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
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           intake2,       tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           lrFlywheel,    tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           urFlywheel,    tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           llFlywheel,    tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           ulFlywheel,    tmotorVex393TurboSpeed_MC29, openLoop, reversed)
//*!!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!
int motorSpeed = 0;
int velocity;
int position1;
int position2;

task flywheelVelocity(){

	while(true){

		position1 = nMotorEncoder(lrFlywheel);
		wait1Msec(200);
		position2 = nMotorEncoder(lrFlywheel);
		wait1Msec(200);
		velocity = (position2 - position1)/4;

	}
}


void speedUpFlywheel(){
	while(motorSpeed < 90)
	if(motorSpeed < 90)
		motorSpeed += 2;
	else
		motorSpeed = 90;

	motor[urFlywheel] = motorSpeed;
	motor[lrFlywheel] = motorSpeed;
	motor[ulFlywheel] = motorSpeed;
	motor[llFlywheel] = motorSpeed;

	wait1Msec(150);
}
}

void slowDownFlywheel(){
	if(motorSpeed > 0)
		motorSpeed -= 2;
	else
		motorSpeed = 0;

	motor[urFlywheel] = motorSpeed;
	motor[lrFlywheel] = motorSpeed;
	motor[ulFlywheel] = motorSpeed;
	motor[llFlywheel] = motorSpeed;

	wait1Msec(150);
}

task shooter(){
	while(true){
		if(vexRT(Btn6U)){
			while(!vexRT(Btn6D))
				speedUpFlywheel();
		}
		else if(vexRT(Btn6D)){
			while(!vexRT(Btn6U))
				slowDownFlywheel();
		}
	}
}


void pre_auton()
{
	bStopTasksBetweenModes = true;
}


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

task usercontrol()
{
	startTask(shooter);
	startTask(flywheelVelocity);
	while (true)
	{

		if(vexRT(Btn5U))
			motor[intake2] = 127;
		else if(vexRT(Btn5D))
			motor[intake2] = -127;
		else
			motor[intake2] = 0;
	}

}