Teleop.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorC,          autoArm,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     leftDrive,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     winch,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     flag,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     unused2,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     rightDrive,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     collector,     tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#include "constants.c"
#include "lib/FTC_ValueUtils.h"
#include "subsystems/Winch.c"
#include "subsystems/FlagRaiser.c"
#include "subsystems/Collector.c"

task main()
{
	waitForStart();
	while (true)
	{
		getJoystickSettings(joystick);

		// Flag OI

		if ( joy2Btn(6) )
			motor[flag] = 20;
		else if ( joy2Btn(5) )
			motor[flag] = -20;
		else
			setFlagRaiserMotor(joy2Btn(2));

		// Winch+Collector OI
		setWinchMotor(dbc(joystick.joy2_y1, 10));
		setCollectorRoller(joy2Btn(7)||joy2Btn(8), joy2Btn(8));

		// Drivetrain OI
		motor[leftDrive] = dbc(-joystick.joy1_y1, 8);
		motor[rightDrive] = dbc(joystick.joy1_y2, 8);

		// Auto arm OI
		motor[autoArm] = dbc(joystick.joy2_y2, 8)/2;
	}
}