map.h

#pragma once

#include <Servo.h>
#include <Spark.h>
#include <CANTalon.h>
#include <Joystick.h>
#include <DigitalInput.h>
#include <AHRS.h>

#include <inttypes.h>

#include "control/lease.h"

#define DRIVE_MOTOR_COUNT 4

namespace Map {
	void init();

	struct Motors {
		/**
		 * Main Swerve Drive Motors, used for translational velocity
		 * #type -> CANTalon (CAN)
		 */
		static constexpr int drivePorts[DRIVE_MOTOR_COUNT] = {10, 11, 12, 13};
		static ControlLease<CANTalon> driveMotors[DRIVE_MOTOR_COUNT];

		/**
		 * Align Swerve Drive Motors, used for rotational velocity
		 * #type -> CANTalon (CAN)
		 */
		static constexpr int alignPorts[DRIVE_MOTOR_COUNT] = {14, 15, 16, 17};
		static ControlLease<CANTalon> alignMotors[DRIVE_MOTOR_COUNT];

		/**
		 * Configuration for Drive Align Motors (e.g. encoder ticks, PID).
		 * This is stored under Motors as the encoders are attached to the
		 * motor controllers (Quadrature on SRX)
		 */
		struct DriveConfig {
			static constexpr double P = 5, I = 0.01, D = 0;
			// PG71 with Encoder Kit: 7 ticks per rev on a 71:1 reduction = 497 ticks per rev
			// 50:37 reduction on swerve module
			static const uint16_t ticksPerRev = (int)ceil(497.0f * (50.0f/37.0f));
		};

		/**
		 * Fuel Feeder Motor
		 * #type -> CANTalon (CAN)
		 */
		static const int feederPort = 18;
		static ControlLease<CANTalon> feederMotor;

		/**
		 * Fuel Launcher Motor
		 * #type -> CANTalon (CAN)
		 */
		static const int launcherPort = 19;
		static ControlLease<CANTalon> launcherMotor;

		/**
		 * Fuel Intake Motor
		 * #type -> Spark (PWM)
		 */
		static const int intakePort = 0;
		static ControlLease<Spark> intakeMotor;

		/**
		 * Climbing Winch Motor
		 * #type -> Spark (PWM)
		 */
		static const int winchPort = 1;
		static ControlLease<Spark> winchMotor;

		/**
		 * Climbing Winch Ratchet Servo
		 * #type -> Servo (PWM)
		 */
		static const int ratchetPort = 2;
		static ControlLease<Servo> ratchetServo;
	};

	struct Sensors {
		/**
		 * Hall Effect Sensors for zero'ing the swerve modules. These are mounted on the
		 * robot chassis and detect a Hard Drive Magnet mounted on the rotational swerve
		 * module.
		 * #type -> Hall Effect Sensor (Digital Input)
		 */
		static constexpr int swerveZeroPorts[DRIVE_MOTOR_COUNT] = {0, 1, 2, 3};
		static DigitalInput *swerveZeroSensors[DRIVE_MOTOR_COUNT];

		/**
		 * NavX Inertial Measurement Unit
		 * #type -> Multiple (USB, I2C, SPI, MXP), hence auto
		 */
		static const auto navxPort = SerialPort::Port::kUSB;
		static AHRS *navx;
	};

	struct Joysticks {
		// Main Drive Joystick USB ID
		static const int drivePort = 0;
		static Joystick *driveJoystick;

		/**
		 * Drive Joystick Button and Axes Bindings
		 */
		struct DriveBind {
			static const int zeroWheelsButton = 11;

			static const int throttleDownBind = 3;
			static const int throttleUpBind = 4;
			static const int feederBind = 2;
			static const int activateLauncherBind = 1;

			static const int winchDownBind = 5;
			static const int winchUpBind = 6;
			static const int ratchetBind = 2;
		};

		// Auxiliary Joystick USB ID
		static const int auxPort = 1;
		static Joystick *auxJoystick;
	};
}

zeroWheelsCmd.h

#include "align/zeroWheelsCmd.h"
#include "control/bind.h"
#include "control/map.h"

#include <iostream>

static bool command_running[4] = { false, false, false, false };

static Lease lease = { ZERO_WHEELS_PRIORITY };  // 1000

void ZeroWheelsCmd::init(CommandExecutor::Executor *exec) {
	ButtonBind::on(Map::Joysticks::driveJoystick, Map::Joysticks::DriveBind::zeroWheelsButton, ButtonBind::PRESSED, [exec](int edge) {
		for (int i = 0; i < 4; i++) {
			exec->push(new ZeroWheelsCmd(Map::Sensors::swerveZeroSensors[i], &Map::Motors::alignMotors[i], &command_running[i]));
		}
	});
}

ZeroWheelsCmd::ZeroWheelsCmd(DigitalInput *din, ControlLease<CANTalon> *ctrl, bool *run_state) {
	timeout(3000);
	input = din;
	control = ctrl;
	is_running = run_state;
}

void ZeroWheelsCmd::start() {
	*is_running = true;

	if (control->IsMine(&lease)) {
		control->Get()->SetControlMode(CANTalon::kPercentVbus);
		control->Get()->EnableControl();
	}
}

void ZeroWheelsCmd::periodic() {
	if (control->IsMine(&lease)) {
		control->Get()->Set(1.0);
	}
}

bool ZeroWheelsCmd::complete() {
	if (!input->Get()) return true;
	return false;
}

void ZeroWheelsCmd::stop() {
	if (control->IsMine(&lease)) {
		control->Get()->Set(0.0);
		control->Get()->SetPosition(0.0);
		control->Get()->SetControlMode(CANTalon::kPosition);
		control->Get()->EnableControl();
	}
	control->Release(&lease);
	*is_running = false;
}

bool ZeroWheelsCmd::can_start() {
	return input->Get() && !(*is_running);
}