KombatWombat_comp_end_game.c

#pragma config(Sensor, dgtl1,  clawEncoder,    sensorRotation)
#pragma config(Sensor, dgtl2,  driveEncoderLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl4,  driveEncoderRight, sensorQuadEncoder)
#pragma config(Motor,  port1,           harvester_1,   tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port3,           right_drive,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           left_drive,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           right_drive_2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           left_drive_2,  tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition Code for Kombat Wombat - Trinity College  */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"

///////////Controllers///////////
#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;

int clawEncoderValue;


/*//////////Lift///////////
void SetLiftPower(int power){
	motor[top_lift] = power;
	motor[bottom_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;
}

/* float get_joystick_axis_y(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(DriveForward));
	float steer_power = get_power(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);
	set_motor_speed(right_drive_2, right_power);
	set_motor_speed(left_drive_2, left_power);
}

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

//Autonomous varables
float circumferenceWheel = 0.916;
float RobotOneDeg = 3.403;
int LeftEncorderCount = 0;
int RightEncoderCount = 0;

void MoveForward(float distanceMM){
	SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	bool state = true;
	int NumCounts = (int)(distanceMM/circumferenceWheel);
	int direction = 1;
	if(distanceMM < 0){
		direction = -1;
	}
	while (state){
		LeftEncorderCount = SensorValue[driveEncoderLeft];
		RightEncoderCount = SensorValue[driveEncoderRight];
		if((LeftEncorderCount+RightEncoderCount)/2<NumCounts*direction){
			motor[left_drive] = 127*direction;
			motor[left_drive_2] = 127*direction;
			motor[right_drive] = 84*direction;
			motor[right_drive_2] = 84*direction;
		}
		else{
			motor[left_drive] = 0;
			motor[left_drive_2] = 0;
			motor[right_drive] = 0;
			motor[right_drive_2] = 0;
			SensorValue[driveEncoderLeft] = 0;
			SensorValue[driveEncoderRight] = 0;
			state = false;
		}
	}
}
void TurnRobot(int degree){
	SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	float turingDistance = degree*RobotOneDeg;
	int NumCounts = (int)(turingDistance/circumferenceWheel);
	int direction = 1;
	if(degree<0){
		direction = -1;
	}
	bool state = true;
	while (state){
		LeftEncorderCount = SensorValue[driveEncoderLeft];
		RightEncoderCount = SensorValue[driveEncoderRight];
		if(LeftEncorderCount < NumCounts*direction){
			motor[left_drive] = 127*direction;
			motor[left_drive_2] = 127*direction;
			motor[right_drive] = -84*direction;
			motor[right_drive_2] = -84*direction;
		}
		else{
			motor[left_drive] = 0;
			motor[left_drive_2] = 0;
			motor[right_drive] = 0;
			motor[right_drive_2] = 0;
			SensorValue[driveEncoderLeft] = 0;
			SensorValue[driveEncoderRight] = 0;
			state = false;
		}
	}
}

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks
	// running between Autonomous and Driver controlled modes. You will need to
	// manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
	SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...



}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{
	// ..........................................................................
	// Insert user code here.
	// ..........................................................................

	// Remove this function call once you have "real" code.
	//AutonomousCodePlaceholderForTesting();

  /////Platform/////
  /*sleep(500);
	SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	sleep(500);
  MoveForward(400);
  sleep(500);
  SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	sleep(500);
	TurnRobot(80);
	sleep(500);
	SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	sleep(500);
	MoveForward(900);
	*/

////Flags//////
	sleep(500);
  SensorValue[driveEncoderLeft] = 0;
	SensorValue[driveEncoderRight] = 0;
	sleep(500);
	MoveForward(1100);

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
	// User control code here, inside the loop

	while (true)
	{
		// This is the main execution loop for the user control program.
		// Each time through the loop your program should update motor + servo
		// values based on feedback from the joysticks.

		// ........................................................................
		// Insert user code here. This is where you use the joystick values to
		// update your motors, etc.
		// ........................................................................

		////Assign Button///////
		bool BtnArmUp = (bool)vexRT[ArmUp];
		bool BtnArmDown = (bool)vexRT[ArmDown];

		update_drive();

		clawEncoderValue = SensorValue[clawEncoder];

		//////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);
		}
		*/

	}
}