Program for controlling 5 motors with an XBOX360 controller connected via USB shield.

ROV.ino

/**
 * Copyright (c) 2017 Alex White, Jeremy Harris, Abe Zukor
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#define VERSION "0.5"

/** Configuration: Serial **/
#define SERIAL_RATE 250000 //Data rate.

/** Configuration: Debug **/
#define DEBUG_INPUT  0 //Print values read from controllers.
#define DEBUG_OUTPUT 1 //Print values sent to motors.

/** Configuration: Controls **/
#define CONTROLS_DEADBAND 0.06f //Joystick values closer than this to zero will be ignored.

/** Configuration: Pinouts **/
#define PIN_MOTOR_1 2 //Left fwd/back motor.
#define PIN_MOTOR_2 3 //Right fwd/back motor.
#define PIN_MOTOR_3 4 //Front up/down motor.
#define PIN_MOTOR_4 5 //Back up/down motor.
#define PIN_MOTOR_5 6 //Center sideways motor.

/** Configuration: Motors **/
#define MOTORS_ADD 500  //Maximum value to add or subtract from MOTORS_OFF.
#define MOTORS_OFF 1500 //Motors off signal.


#include <Wire.h>
#include <Servo.h>
#include <XBOXUSB.h>
#include <limits.h>

enum input_t {
	L_STICK_X,
	L_STICK_Y,
	R_STICK_X,
	R_STICK_Y,
	TRIGGERS,
	BUMPERS,
	DPAD_X,
	DPAD_Y,
	NONE,
};

const struct control_map_t {
	input_t fwd_back;
	input_t turning;
	input_t up_down;
	input_t sideways;
} ctrl_map = {
	.fwd_back = R_STICK_Y,
	.turning  = L_STICK_X,
	.up_down  = L_STICK_Y,
	.sideways = R_STICK_X,
};

Servo motor1, motor2, motor3, motor4, motor5;
USB usb;
XBOXUSB xbox(&usb);

void setup() {
	Serial.begin(SERIAL_RATE);
	const char *init_msg = "This is version " VERSION ", compiled on " __DATE__ ".\n"
	                       #if DEBUG_INPUT
	                       "Config: Debugging input is enabled.\n"
	                       #endif
	                       #if DEBUG_OUTPUT
	                       "Config: Debugging output is enabled.\n"
	                       #endif
	                       "";
	Serial.println(init_msg);

	Serial.print("Initializing USB... ");
	while (usb.Init() == -1)
		delay(1000);

	Serial.println("done.\n");

	const int m_min = MOTORS_OFF - MOTORS_ADD;
	const int m_max = MOTORS_OFF + MOTORS_ADD;
	motor1.attach(PIN_MOTOR_1, m_max, m_min);
	motor2.attach(PIN_MOTOR_2, m_max, m_min);
	motor3.attach(PIN_MOTOR_3, m_max, m_min);
	motor4.attach(PIN_MOTOR_4, m_max, m_min);
	motor5.attach(PIN_MOTOR_5, m_max, m_min);
}

void loop() {
	usb.Task();
	float fwd_back = get_input_val(ctrl_map.fwd_back);
	float up_down  = get_input_val(ctrl_map.up_down);
	float turning  = get_input_val(ctrl_map.turning);
	float sideways = get_input_val(ctrl_map.sideways);
	#if DEBUG_INPUT
	char out_i_str[85];
	sprintf(out_i_str, "[Debug] Input: fwd/back: %5d, turning: %5d, up/down: %5d, sideways: %5d", int (fwd_back*1000), int (turning*1000), int (up_down*1000), int (sideways*1000));
	Serial.println(out_i_str);
	#endif
	int speed1, speed2, speed3, speed4, speed5;
	speed1 = convert_and_limit(fwd_back + turning);
	speed2 = convert_and_limit(fwd_back - turning);
	speed3 =
	speed4 = convert_and_limit(up_down);
	speed5 = convert_and_limit(sideways);
	#if DEBUG_OUTPUT
	char out_o_str[64];
	sprintf(out_o_str, "[Debug] Output: M1: %3d, M2: %3d, M3: %3d, M4: %3d, M5: %3d", speed1, speed2, speed3, speed4, speed5);
	Serial.println(out_o_str);
	#endif
	motor1.writeMicroseconds(speed1);
	motor2.writeMicroseconds(speed2);
	motor3.writeMicroseconds(speed3);
	motor4.writeMicroseconds(speed4);
	motor5.writeMicroseconds(speed5);
}

float get_input_val(input_t input) {
	int raw_value;
	if(xbox.Xbox360Connected) {
		if(input == L_STICK_X)
			raw_value = xbox.getAnalogHat(LeftHatX);
		else if(input == L_STICK_Y)
			raw_value = xbox.getAnalogHat(LeftHatY);
		else if(input == R_STICK_X)
			raw_value = xbox.getAnalogHat(RightHatX);
		else if(input == R_STICK_Y)
			raw_value = xbox.getAnalogHat(RightHatY);
		else if(input == TRIGGERS)
			raw_value = (xbox.getButtonPress(L2) - xbox.getButtonPress(R2)) * 128;
		else if(input == BUMPERS)
			raw_value = (xbox.getButtonPress(L1) - xbox.getButtonPress(R1)) * INT_MAX;
		else if(input == DPAD_X)
			raw_value = (xbox.getButtonPress(LEFT) - xbox.getButtonPress(RIGHT)) * INT_MAX;
		else if(input == DPAD_Y)
			raw_value = (xbox.getButtonPress(UP) - xbox.getButtonPress(DOWN)) * INT_MAX;
		else
			raw_value = 0;
		if(raw_value == INT_MIN)
			raw_value++;
	} else {
		raw_value = 0;
	}
	float value = raw_value * (1.0f / INT_MAX);
	return abs(value) > CONTROLS_DEADBAND ? value : 0.0f;
}

int convert_and_limit(float value) {
	if(value > 1.0f)
		value = 1.0f;
	else if(value < -1.0f)
		value = -1.0f;
	return value * MOTORS_ADD + MOTORS_OFF;
}