Robotics/robo2019.c at master · stevenraphael/Robotics · 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
#pragma config(Sensor, dgtl1,  Sol_A,          sensorDigitalOut)
#pragma config(Sensor, dgtl2,  Sol_B,          sensorDigitalOut)
#pragma config(Motor,  port2,           Claw,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           Arm,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           backRight,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           backLeft,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           frontRight,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           frontLeft,     tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	//Loop Forever
	while(1 == 1)
	{
		/*if (vexRT[Btn5UXmtr2] == 1){
			SensorValue(Sol_B) = 0;
			SensorValue(Sol_A) = 1;

			}else{
			SensorValue(Sol_B) = 1;
			SensorValue(Sol_A) = 0;*/


			//Remote Control Commands (dual joystick drive for wheel)
			if (abs(vexRT[Ch2]) > 15 || abs(vexRT[Ch3]) > 15 || vexRT[Btn7L] == 1 || vexRT[Btn8R] == 1){
				motor[frontRight] = vexRT[Ch2] * 0.8 - vexRT[Btn7L] * 60 + vexRT[Btn8R] * 60;
				motor[frontLeft] = vexRT[Ch3] * 0.8 + vexRT[Btn7L] * 60 - vexRT[Btn8R] * 60;
				motor[backRight] = vexRT[Ch2] * 0.8 + vexRT[Btn7L] * 60 - vexRT[Btn8R] * 60;
				motor[backLeft] = vexRT[Ch3] * 0.8 - vexRT[Btn7L] * 60 + vexRT[Btn8R] * 60;
				//motor[Conv] = vexRT[Btn6U] * 70 + vexRT[Btn6D] * (-70);
				//
				//motor[FW2] = vexRT[Btn6UXmtr2] * 256;
			}else{
				motor[frontRight] = 0;
				motor[backRight] = 0;
				motor[frontLeft] = 0;
				motor[backLeft] = 0;
				//motor[Conv] = 0;
				//motor[FW1] = 0;
				//motor[FW2] = 0;
			}
			//motor[Intake] = vexRT[Btn7UXmtr2] * 256 - vexRT[Btn7DXmtr2] * 256;
			//motor[Arm]=vexRT[Ch3Xmtr2] * 0.8;
			if(vexRT[Btn6U]+vexRT[Btn6UXmtr2]+vexRT[Btn6D]+vexRT[Btn6DXmtr2]>=2){
				motor[Claw]=15;
			}
			else{
				motor[Claw]=100*(vexRT[Btn6U]+vexRT[Btn6UXmtr2])-100*(vexRT[Btn6D]+vexRT[Btn6DXmtr2]);
			}
			if(vexRT[Btn5U]==1||vexRT[Btn5UXmtr2]==1){
				motor[Arm]=-50; //This value probably has to be changed
			}
			else{
				if(vexRT[Ch3Xmtr2]>15){
					motor[Arm]=vexRT[Ch3Xmtr2] * -0.8;
				}
				else{
					motor[Arm]=vexRT[Btn7U] * (-170)-15*vexRT[Btn7D]-15*vexRT[Btn7DXmtr2];
				}
			}
		}
	}

/* Connections Table
Victor Number    VEX Brain Port
1									 6
2									 7
3									 8
4									 9
5									 2
6									 3
7									 4
8									 5
*/