VexIQ Triple Play Team 800 Code

Chrisbot.c

#pragma config(Sensor, port2,  statusLED,      sensorVexIQ_LED)
#pragma config(Motor,  motor1,          leftMotor,     tmotorVexIQ, openLoop, driveLeft)
#pragma config(Motor,  motor6,          wristMotor,    tmotorVexIQ, openLoop, encoder)
#pragma config(Motor,  motor7,          rightMotor,    tmotorVexIQ, openLoop, reversed, driveRight)
#pragma config(Motor,  motor9,          fourBarMotor,  tmotorVexIQ, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
 * IntakeAssembly control modes.
 */
typedef enum ControlMode {
	Idle,   /**< Idle mode, no movement or current control mode. */
	Manual, /**< Manual mode, driver manually moving IntakeAssembly. */
	Preset  /**< Preset mode, reading an IntakePreset. */
};

/**
 * FourBar presets.
 */
enum FourBarPreset {
	FOURBAR_GROUND = 0,     /**< FourBar ground preset. */
	FOURBAR_STOW = 30,      /**< FourBar stow preset. */
	FOURBAR_LOW_GOAL = 300, /**< FourBar low goal preset. */
	FOURBAR_HIGH_GOAL = 500 /**< FourBar high goal preset. */
};

/**
 * Wrist presets.
 */
enum WristPreset {
	WRIST_GROUND = 0,   /**< Wrist ground preset. */
	WRIST_STOW = 70,    /**< Wrist stow preset. */
	WRIST_COMPACT = 100 /**< Wrist compact preset. */
};

/**
 * Struct that contains a FourBar and Wrist preset.
 */
typedef struct{
	float fourBarPosition; /**< The FourBar position. */
	float wristPosition;   /**< The Wrist position. */
} IntakePreset;


IntakePreset m_intakePosGoal; /**< IntakeAssembly preset goal. */
ControlMode m_controlMode;    /**< IntakeAssembly control mode. */

float m_fourBarOutput = 0.0; /**< FourBar output. */
float m_wristOutput = 0.0;   /**< Wrist output. */

int m_wristTravelTimer = 0; /**< Wrist travel timer. */

float FOURBAR_MAX_SPEED = 100.0; /**< Maximum FourBar speed. */
float WRIST_MAX_SPEED = 25.0;    /**< Maximum Wrist speed. */

IntakePreset GROUND_PRESET;    /**< IntakeAssembly ground preset. */
IntakePreset STOW_PRESET;      /**< IntakeAssembly stow preset. */
IntakePreset COMPACT_PRESET;   /**< IntakeAssembly compact preset. */
IntakePreset LOW_GOAL_PRESET;  /**< IntakeAssembly low goal preset. */
IntakePreset HIGH_GOAL_PRESET; /**< IntakeAssembly high goal preset. */


/**
 * Return the lesser of the two given numbers.
 * @param a The first number.
 * @param b The second number.
 * @return The lesser number.
 */
float min(float a, float b) {
	if (a < b) {
		return a;
	}
	else {
		return b;
	}
}

/**
 * Return the greater of the two given numbers.
 * @param a The first number.
 * @param b The second number.
 * @return The greater number.
 */
float max(float a, float b) {
	if (a > b) {
		return a;
	}
	else {
		return b;
	}
}

/**
 * Return the limit of the two given numbers.
 * @param x
 * @param maxMagnitude
 * @return The limit of the numbers.
 */
float limit(float x, float maxMagnitude) {
	return min(maxMagnitude, max(-maxMagnitude, x));
}

/**
 * Square the given number, but keep the sign the same.
 * @param n The number.
 * @return The square of the number with the same sign.
 */
float signSquare(float n) {
	if (n < 0.0) {
		return -1.0 * n * n;
	}
	else {
		return n * n;
	}
}

/**
 * Teleop initialization.
 */
task TeleopInit() {
	setTouchLEDColor(statusLED, colorOrange);

	setMotorBrakeMode(leftMotor, motorCoast);
	setMotorBrakeMode(rightMotor, motorCoast);
	setMotorBrakeMode(fourBarMotor, motorCoast);
	setMotorBrakeMode(wristMotor, motorHold);

	GROUND_PRESET.fourBarPosition = FOURBAR_GROUND;
	GROUND_PRESET.wristPosition = WRIST_GROUND;

	STOW_PRESET.wristPosition = WRIST_STOW;

	COMPACT_PRESET.wristPosition = WRIST_COMPACT;

	LOW_GOAL_PRESET.fourBarPosition = FOURBAR_LOW_GOAL;
	LOW_GOAL_PRESET.wristPosition = WRIST_GROUND;

	HIGH_GOAL_PRESET.fourBarPosition = FOURBAR_HIGH_GOAL;
	HIGH_GOAL_PRESET.wristPosition = WRIST_GROUND;

	m_controlMode = Idle;
}

/**
 * Periodic teleop loop.
 */
task TeleopPeriodic() {
	
 	/**
 	 * Drive mode control.
 	 */
 	
 	// Get the joystick values.
	float y = -getJoystickValue(ChA);    /**< Current y joystick value. */
	float x = getJoystickValue(ChC);     /**< Current x joystick value. */
	int slow = getJoystickValue(BtnRUp); /**< Current slow mode joystick value/ */
	
	// Change joystick value to a decimal.
	y /= 100;
	x /= 100;
	
	// Do some nice sine stuff to ramp up driving.
	y = sin(pow(y, 3));
	x = sin(pow(x, 3));
	
	// Halve if slow mode.
	if (slow == 1) {
		y *= 0.5;
		x *= 0.5;
	}
	
	// Change to a percentage.
	y *= 100;
	x *= 100;
	
	// Set the motor speed.
	setMotorSpeed(leftMotor, y - x);
	setMotorSpeed(rightMotor, y + x);
	
	
	/**
	 * IntakeAssembly control.
	 */
	
	// Handle presets.
	if (getJoystickValue(BtnLUp) == 1) {
		m_controlMode = Preset;
		m_intakePosGoal.fourBarPosition = HIGH_GOAL_PRESET.fourBarPosition;
		m_intakePosGoal.wristPosition = STOW_PRESET.wristPosition;
		m_wristTravelTimer = time1[T1];
	}
	
	if (getJoystickValue(BtnLDown) == 1) {
		m_controlMode = Preset;
		m_intakePosGoal.fourBarPosition = GROUND_PRESET.fourBarPosition;
		m_intakePosGoal.wristPosition = STOW_PRESET.wristPosition;
		m_wristTravelTimer = time1[T1];
	}

	
	// Handle wrist stow timing.
	if (time1[T1] - m_wristTravelTimer > 500) {
		m_intakePosGoal.wristPosition = GROUND_PRESET.wristPosition;
	}

	
	// Handle FourBar manual control.
	if (getJoystickValue(BtnEUp) == 1) {
		m_controlMode = Manual;
		m_fourBarOutput = FOURBAR_MAX_SPEED;
	}
	if (getJoystickValue(BtnEDown) == 1) {
		m_controlMode = Manual;
		m_fourBarOutput = -FOURBAR_MAX_SPEED;
	}
	if (getJoystickValue(BtnEUp) == 0 && getJoystickValue(BtnEDown) == 0) {
		m_fourBarOutput = 0;
	}
	
	// Handle FourBar manual control.
	if (getJoystickValue(BtnFUp) == 1) {
		m_controlMode = Manual;
		m_wristOutput = WRIST_MAX_SPEED;
	}
	if (getJoystickValue(BtnFDown) == 1) {
		m_controlMode = Manual;
		m_wristOutput = -WRIST_MAX_SPEED;
	}
	if (getJoystickValue(BtnFUp) == 0 && getJoystickValue(BtnFDown) == 0) {
		m_wristOutput = 0;
	}
	
	// Handle IntakeAssembly control modes.
	switch (m_controlMode) {
	case Idle:
		setTouchLEDColor(statusLED, colorOrange);

		setMotorSpeed(fourBarMotor, 0);
		setMotorSpeed(wristMotor, 0);
		break;
	case Manual:
		setTouchLEDColor(statusLED, colorBlue);

		setMotorBrakeMode(fourBarMotor, motorBrake);
		setMotorBrakeMode(wristMotor, motorCoast);

		setMotorSpeed(fourBarMotor, m_fourBarOutput);
		setMotorSpeed(wristMotor, m_wristOutput);

		break;
	case Preset:
		setTouchLEDColor(statusLED, colorGreen);

		setMotorBrakeMode(fourBarMotor, motorHold);

		setMotorTarget(fourBarMotor, m_intakePosGoal.fourBarPosition, FOURBAR_MAX_SPEED);
		setMotorTarget(wristMotor, m_intakePosGoal.wristPosition, WRIST_MAX_SPEED);
		break;
	}
	
		
	/**
	 * Debug on the brain.
	 */
	displayString(1, "y: %f", y);
	displayString(2, "x: %f", x);
	displayString(3, "sl: %d", slow);
	displayString(4, "fb e: %f", getMotorEncoder(fourBarMotor));
	displayString(5, "w e: %f", getMotorEncoder(wristMotor));
}

/**
 * Main task.
 */
task main() {
	// Start teleop initialization.
	startTask(TeleopInit);

	while (true) {
		// Run teleop periodic.
		startTask(TeleopPeriodic);
	}
}