KombatWombat.c

#pragma config(Motor,  port1,           harvester_2,   tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           left_drive,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           right_drive,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           launch_motor_1, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           launch_motor_2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           claw_roll,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           top_lift,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           bottom_lift,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           claw_pitch,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          harvester_1,   tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define LaunchForwards Btn5U
#define LaunchBackwards Btn5D
#define DriveForward Ch3
#define DriveSteer Ch1
#define ArmUp Btn6U
#define ArmDown Btn6D
#define HarvesterToggle Btn7UXmtr2
#define Harvester2 Btn7DXmtr2
#define ClawRotateLeft Btn8LXmtr2
#define ClawRotateRight Btn8RXmtr2
#define ClawPitchUp Btn8UXmtr2
#define ClawPitchDown Btn8DXmtr2

#define sign(value) (value >= 0 ? 1 : -1)
#define max(a,b) (a > b ? a : b)


///////////Variables///////////
bool LastButtonState = false;
bool CurrentButtonState = false;

void SetLiftPower(int power){
	motor[top_lift] = power;
	motor[bottom_lift] = power;

}

///////////Drive///////////
float get_power_forward(float controller_input)
{
	float controller_sign = sign(controller_input);
	float new_input = controller_input * controller_sign;

	new_input = max(0.0, new_input - 0.15) / (1.0 - 0.15);
	return controller_sign * new_input;
}

float get_power_turning(float controller_input)
{
	float controller_sign = sign(controller_input);
	float new_input = controller_input * controller_sign;

	new_input = max(0.0, new_input - 0.15) / (1.0 - 0.15);
	return controller_sign * new_input;
}

float get_joystick_axis(int index)
{
	float maximum_range = vexRT[index] > 0 ? -127.0 : -128.0;
	float joy_pos = (float)vexRT[index];
	return joy_pos / maximum_range;
}

void set_motor_speed(int _motor, float speed)
{
	if (speed > 1.0) speed = 1.0;
	if (speed < -1.0) speed = -1.0;
	int maximum_speed = 127;
	int motor_speed = maximum_speed * speed;
	motor[_motor] = motor_speed;
}

void update_drive () {
	float forward_power = get_power_forward(get_joystick_axis(DriveForward));
	float steer_power = get_power_turning(get_joystick_axis(DriveSteer));

	float left_power = forward_power + steer_power;
	float right_power = forward_power - steer_power;

	set_motor_speed(left_drive, left_power);
	set_motor_speed(right_drive, right_power);
}


task main()
{
	while(true)
	{
		////Assign Button///////
		bool BtnArmUp = (bool)vexRT[ArmUp];
		bool BtnArmDown = (bool)vexRT[ArmDown];
		bool BtnShooter = (bool)vexRT[LaunchForwards];

		update_drive();

		//////ArmLift//////
		if (BtnArmUp){
			SetLiftPower(127);
		}
		else if (BtnArmDown){
			SetLiftPower(127);
		}
		else {
			SetLiftPower(0);
		}


		//////Shooter////////
		if (BtnShooter && !LastButtonState){
			CurrentButtonState = true;
		}

		if (CurrentButtonState != LastButtonState){
			LastButtonState = CurrentButtonState;

		}
		if (CurrentButtonState){
			motor[launch_motor_1] = 127;
			motor[launch_motor_2] = 127;
		}
		else {
			motor[launch_motor_1] = 0;
			motor[launch_motor_2] = 0;

		}

	}
}