TeleOp.c

#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     limit_switch,   sensorTouch)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     arm_tipper,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     flag_spinner,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     right_drive,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_2,     left_drive,    tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "gearcats_helpers.h"

//disable main tasks in functions from other files
#define MAIN_ROBOT_CODE

const float POWER_LEVEL_1 = 0.50;
const float POWER_LEVEL_2 = 1.00;

float current_power_level;

int forward;
int turn;

//will be run once at start of teleop
void initializeRobot()
{
	left_drive_motor = left_drive;
	right_drive_motor = right_drive;
	current_power_level = POWER_LEVEL_1;
  // Place code here to initialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  return;
}



//zeroes a value if its magnitude is less than a certain value
//used to create a dead zone for the motors
int floor(int input, int min_value){
	if (abs(input) < min_value) {
		return 0;
	} else {
		return input;
	}
}

task drive() {
	while(true){
		getJoystickSettings(joystick);

		//these have to be floored to create a dead zone, preventing motor drift and noise
		forward = floor(joystick.joy1_y1, 10) * current_power_level;
		turn = floor(joystick.joy1_x2,10) * current_power_level;
		if (abs(forward) < 10){
			forward = 0;
		}
		arcade_drive(forward, turn);
		nxtDisplayTextLine(1, "forward: %d", forward);
		nxtDisplayTextLine(2, "turn: %d", turn);
	}
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase
	StartTask(drive);
  while (true)
  {
	  getJoystickSettings(joystick);


	  //power level controls: driver can set motors to always spin at half speed, allowing more precise control
	  if (joy1Btn(A_BTN) == 1){
	  	current_power_level = POWER_LEVEL_1;
	  }
	  if (joy1Btn(B_BTN) == 1) {
	  	current_power_level = POWER_LEVEL_2;
	  }

		int arm_tipper_speed = 0;

	  //move the arm up if X pressed, down if Y, otherwise brake
		if(joy1Btn(X_BTN) == 1) {
			arm_tipper_speed = 20;
		}
		else if(joy1Btn(Y_BTN) == 1) {
			arm_tipper_speed = -20;
		}
		else {
			arm_tipper_speed = 0;
		}

		if (joy1Btn(6)){
			arm_tipper_speed /= 2;
		}

  	motor[arm_tipper] = arm_tipper_speed;

  	if (joy1Btn(5)) {
  		motor[flag_spinner] = 63;
  	} else {
  		motor[flag_spinner] = 0;
  	}
  }

}