InnovationCenterBEST/oldCode.c at master · ebuttonsdude/InnovationCenterBEST · 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
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           LeftBaseBack,  tmotorVex393HighSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port4,           RightBaseFront, tmotorVex393HighSpeed_MC29, openLoop, driveRight)
#pragma config(Motor,  port5,           RightBaseBack, tmotorVex393HighSpeed_MC29, openLoop, driveRight)
#pragma config(Motor,  port6,           ScissorLeft,   tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port7,           ScissorRight,  tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port8,           ArmLeft,       tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port9,           ArmRight,      tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port10,          Claw,          tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
// The program is below
//begin user control code
void UDrive()
{
	//forward and backward
	motor(LeftBaseFront) = vexRT(Ch3);
	motor(LeftBaseBack) = vexRT(Ch3);
	motor(RightBaseBack) = vexRT(Ch3);
	motor(RightBaseFront) = vexRT(Ch3);

	//turning
	if(vexRT(Ch1) < 0)
	{
		motor(LeftBaseFront) = vexRT(Ch3);
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(RightBaseBack) = -abs(vexRT(Ch3));
		motor(RightBaseFront) = -abs(vexRT(Ch3));
	}
	if(vexRT(Ch1) > 0)
	{
		motor(LeftBaseFront) = -abs(vexRT(Ch3));
		motor(LeftBaseBack) = -abs(vexRT(Ch3));
		motor(RightBaseBack) = vexRT(Ch3);
		motor(RightBaseFront) = vexRT(Ch3);
	}
	//side to side
	motor(LeftBaseBack) = -vexRT(Ch4);
	motor(LeftBaseFront) = vexRT(Ch4);
	motor(RightBaseBack) = vexRT(Ch4);
	motor(RightBaseFront) = -vexRT(Ch4);
}

void UScissor()
{
	motor[ScissorLeft] = vexRT[Ch3Xmtr2] + 10;
	motor[ScissorRight] = vexRT[Ch3Xmtr2] + 10;
}

void UArms()
{
	motor[ArmLeft] = vexRT[Ch2Xmtr2] + 10;
	motor[ArmRight] = vexRT[Ch2Xmtr2] + 10;
}


void UClaw()
{
	if(vexRT[Btn6UXmtr2] == 1)
	{
		motor[Claw] = 127;
	}
	if(vexRT[Btn6DXmtr2] == 1)
	{
		motor[Claw] = -127;
	}
	if(vexRT[Btn6UXmtr2] == 0 && vexRT[Btn6DXmtr2] == 0)
	{
		motor[Claw] = 0;
	}
}


task main()
{
	while(1)
	{
		UDrive();
		UScissor();
		UArms();
		UClaw();
		//This is a TEST!!
		//Hello World!
	}
}