SaveFishies2018/TestCode.c at master · BNTH-Robotics/SaveFishies2018 · 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
#pragma config(Motor,  port2,           sm1,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           sm2,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           lm1,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           lm2,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           svo1,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port7,           svo2,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           svo3,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port9,           svo4,          tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	motor[svo1]=-127;
	motor[svo2]=-127;
	motor[svo3]=-127;
	motor[svo4]=-127;
while(1==1)
	{
		motor[sm1]=vexRT[Ch3];
		motor[sm2]=vexRT[Ch2];
		motor[lm1]=vexRT[Ch4];
		motor[lm2]=vexRT[Ch1];

		if(vexRT[Btn8L]==1)
			{
				motor[svo1]=127;
			}
		if(vexRT[Btn8U]==1)
			{
				motor[svo2]=127;
			}
		if(vexRT[Btn8D]==1)
			{
				motor[svo3]=127;
			}
		if(vexRT[Btn8R]==1)
			{
				motor[svo4]=127;
			}
		if(vexRT[Btn7L]==1)
			{
				motor[svo1]=-127;
			}
		if(vexRT[Btn7U]==1)
			{
				motor[svo2]=-127;
			}
		if(vexRT[Btn7D]==1)
			{
				motor[svo3]=-127;
			}
		if(vexRT[Btn7R]==1)
			{
				motor[svo4]=-127;
			}

		while((vexRT[Btn5D]==1)&& (vexRT[Btn6D]==1))
			{

				motor[sm1]=127;
				motor[sm2]=127;
				motor[lm1]=127;
				motor[lm2]=127;

				motor[svo1]=-127;
				motor[svo2]=-127;
				motor[svo3]=-127;
				motor[svo4]=-127;
	wait1Msec(500);
				motor[svo1]=127;
				motor[svo2]=127;
				motor[svo3]=127;
				motor[svo4]=127;


			}
	}
}