BaseDrive/HolonomicDriveTestV1.c at master · team62/BaseDrive · 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
#pragma config(Motor,  port1,           rightWheel1,   tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           leftWheel2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           leftWheel1,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          rightWheel2,   tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#define C1LX vexRT[Ch4]
#define C1LY vexRT[Ch3]
#define C1RX vexRT[Ch1]

void pre_auton()
{
  bStopTasksBetweenModes = true;

}

task autonomous()
{

}

task usercontrol()
{
	// User control code here, inside the loop

	while (true)
	{
    // Y component, X component, Rotation
		motor[leftWheel1]   = -C1LY - C1LX - C1RX;
		motor[rightWheel1]  =  C1LY - C1LX - C1RX;
		motor[rightWheel2]  =  C1LY + C1LX - C1RX;
		motor[leftWheel2]   = -C1LY + C1LX - C1RX;

		//Anywhere from 25-50 Msec pause
		wait1Msec(30);
	}
}