KombatWombat_2.c5.c

#pragma config(Motor,  port1,           harvester_1,   tmotorVex393_HBridge, openLoop, reversed)
#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,           left_lift,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           right_lift,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           claw_pitch,    tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

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


///////////Variables///////////
bool lastButtonState = false;
bool state = false;

void SetLiftPower(int power){
	motor[left_lift] = power;
	motor[right_lift] = power;

}

///////////Drive///////////

float get_power(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(get_joystick_axis(DriveSteer));
	float steer_power = get_power(get_joystick_axis(DriveForward));

	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];

		update_drive();

		//////Harvester/////
		bool buttonState = (bool)(vexRT[HarvesterToggle]);
		if (buttonState && !lastButtonState){
			state = !state;
		}
		if (buttonState != lastButtonState){
			lastButtonState = buttonState;
		}
		if (state){
			motor[harvester_1] = 100;

		}
		else {
			motor[harvester_1] = 0;
		}

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

	}
}