ICS4U-PCP/stackofficial at master · kappaross123/ICS4U-PCP · 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
#pragma config(Sensor, port2,  bumper,         sensorVexIQ_Touch)
#pragma config(Sensor, port3,  colour,         sensorVexIQ_Color12Color)
#pragma config(Motor,  motor1,          left,          tmotorVexIQ, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motor6,          right,         tmotorVexIQ, PIDControl, reversed, driveRight, encoder)
#pragma config(Motor,  motor10,         lift,          tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor,  motor11,         claw,          tmotorVexIQ, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


void cubeHolder()
{
	forward(180, degrees, 50);
	moveMotor(claw, 90, degrees, 50);
	moveMotor(lift, 3, rotations, 50);
	moveMotor(claw, 90, degrees, -50);
	moveMotor(lift, 3, rotations, -50);
}
void cubePickUp()
{
	forward(180, degrees, 50);
	moveMotor(claw, 90, degrees, 50);
	moveMotor(lift, 1, rotations, 50);
}
void cubeDrop()
{
	moveMotor(lift, 2, rotations, -50);
	moveMotor(claw, 90, degrees, -50);
}
void cubeHolderDrop()
{
	moveMotor(lift, 3, rotations, 50);
	moveMotor(claw, 90, degrees, 50);
	moveMotor(lift, 3, rotations, -50);
	moveMotor(claw, 90, degrees, -50);
}
void cubePushandStack()
{
	while (getBumperValue(bumper) == 1)
	{
		backward(1, rotations, 50);
		cubeDrop();
		cubeHolderDrop();
	}
}

void checkCubeColor()
{
	bool color;
	while (color)
	{
		if (getColorName(colour) == colorGreen)
		{
			color = false;
		}
		if (getColorName(colour) == colorRed)
		{
			color = false;
			stopAllMotors();
		}
	}
}

task main()
{
	int i = 0;
	while (i != 3)
	{
		displayTextLine(1, "hello world");
		setMultipleMotors(50, left, right);
		while (i == 0)
		{
			checkCubeColor();
			cubeHolder();
			i++;
		}
		while (i == 1)
		{
			checkCubeColor();
			cubePickUp();
			i++;
		}
		while (i == 2)
		{
			cubePushandStack();
			//checkCubeColor();
			i++;
		}
	}
}