RoverBot.c

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftDrive,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     rightDrive,    tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

// Deadband for the driver joysticks
const int DRIVE_JOYSTICK_DB = 5;

//
// Deadband function
//
// Check if the absolute value of the value parameter is less than the deadband
// parameter. If true, return 0; else return the value.
//
// @param value the value to check
// @param db the deadband parameter
// @return 0 if value is in the deadband, else value
//
// Examples:
// int result = deadband(50, 5);	// result == 50
// int result = deadband(2, 5); 	// result == 0
// int result = deadband(-2, 5);	// result == 0
// int result = deadband(-10, 5); // result == -10
//
int deadband(int value, int db)
{
	return abs(value) < db ? 0 : value;
}

//
// main() is where program execution begins
//
task main()
{
  waitForStart();   // wait for start of tele-op phase

  while (true)	// infinite loop
  {
  	// Read the latest joystick data from the field/driverstation
  	getJoystickSettings(joystick);

  	// Read the left and right joystick y axis
  	int left = joystick.joy1_y1;
  	int right = joystick.joy1_y2;

  	// Apply deadband function
  	left = deadband(left, DRIVE_JOYSTICK_DB);
  	right = deadband(right, DRIVE_JOYSTICK_DB);

  	// Drive with the joysticks
  	motor[leftDrive] = left;
  	motor[rightDrive] = right;
  }
}