2017 SMH code.c

#pragma config(Sensor, in1,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in2,    AutonSelect,    sensorPotentiometer)
#pragma config(Sensor, dgtl1,  ClawRightLimit, sensorTouch)
#pragma config(Sensor, dgtl2,  ClawLeftLimit,  sensorTouch)
#pragma config(Motor,  port1,           ClawLeft,      tmotorVex393HighSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           LeftBack,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           RightBack,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           LeftY,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           LeftArm,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           RightArm,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           RightY,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           LeftFront,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           RightFront,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          ClawRight,     tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define joyThresh 10 // This variable controls the size of the deadzone for the joysticks
#define kP 0.5
#define kI 0.0
#define kD 0.0
#define maxIntegral 40
#define maxForward 127,127
#define maxBackward -127,-127
#define maxRight 127,-127
#define maxLeft -127,127
#define baseStop 0,0
#define singleTileTime 1000
#define spin90Time 300
int armState = 0;

#define potStartPoint 460
#define potEndPoint 2800

float target = 0.0; //DO NOT TOUCH DIRECTLY ON PAIN OF BREAKING ARMS

#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(115)

#include "Vex_Competition_Includes.c"


/**
*
* DriveControl moves the drive based upon the inputs from the parameters
* @param leftPower int -128 to 127
* @param rightPower int -128 to 127
*
*/

int mapJoyInt (TVexJoysticks input)
{
    if (abs(vexRT[input]) > joyThresh)
    {
        return vexRT[input];
    }
    return 0;
}

void driveControl (int leftPower, int rightPower)
{
	motor[LeftBack] = leftPower;
	motor[LeftFront] = leftPower;
	motor[RightBack] = rightPower;
	motor[RightFront] = rightPower;
}

void armControl (int power)
{
	motor[RightArm] = power;
	motor[LeftArm] = power;
	motor[RightY] = power;
	motor[LeftY] = power;
}

void clawControl (int power)
{
	//if sensor is on and we still try to move inwards, this will stop the arm.
	//negative values of power mean they are going outward.
	motor[ClawLeft] = power * (power < 0 || !SensorValue[ClawLeftLimit]);
	motor[ClawRight] = power * (power < 0 || !SensorValue[ClawRightLimit]);
}

void resetMotors()
{
    motor[LeftBack] = 0;
    motor[LeftFront] = 0;
    motor[RightBack] = 0;
    motor[RightFront] = 0;
    motor[RightArm] = 0;
    motor[LeftArm] = 0;
    motor[RightY] = 0;
    motor[LeftY] = 0;
    motor[ClawRight] = 0;
    motor[ClawLeft] = 0;
}

void ArmPIDControl (int input)
{
		target = input;
}

task ArmPID()
{
		float integral = 0;
		float prevDiff = 0;
		int armPower = 0;
		while(1)
		{
				float error = target - SensorValue[ArmPot];

				integral += error;

				if(integral > maxIntegral)
				{
						integral = maxIntegral;
				}

				armPower = kP*error + kI * integral + kD * prevDiff;
				if(abs(armPower) > 30 && SensorValue[ArmPot] < potEndPoint && SensorValue[ArmPot] > potStartPoint)
				{
						armControl(armPower);
				}
				prevDiff = error;
				wait1Msec(20);
		}
}

void stateArmControl ()
{
		if(armState == 0)
		{
				target = potStartPoint;
		}
		else if(armState == 1)
		{
				target = potStartPoint + 200;
		}
		else if(armState == 2)
		{
				target = potEndPoint;
		}
}

void button6Switch()
{
		if(vexRT[Btn6U])
		{
			while(vexRT[Btn6U])
		{

		}
		armState++;
		armState = armState%3;
		}
}

/**
*
* mapJoyInt takes the TVexJoysticks input value and a threshold value and returns an integer
* @param TVexJoysticks is a created variable that accepts joystick channels
* @param threshold is an int -128 to 127
*
*/

int mapButtonInt5()
{
		if(vexRT[Btn5D])
		{
        return -1;
    }
    else if(vexRT[Btn5U])
    {
        return 1;
    }
    else
    {
    		return 0;
  	}
}

int mapButtonInt6()
{
		if(vexRT[Btn6D])
		{
    		return -1;
    }
    else if(vexRT[Btn6U])
    {
        return 1;
    }
    else
    {
    		return 0;
  	}
}

void deploy()
{
		startTask(ArmPID);
}

void cubeAuton (bool side)
{
		deploy();
		driveControl(maxForward);
		clawControl(-127);
		wait1Msec(singleTileTime);
		resetMotors();
		clawControl(127);
		wait1Msec(1000);
		if(side)
		{
				driveControl(maxRight);
		}
		else
		{
				driveControl(maxLeft);
		}//
		wait1Msec((int)(spin90Time*1.5));
		driveControl(baseStop);
		wait1Msec(500);
		driveControl(maxBackward);
		wait1Msec(singleTileTime*2);
		driveControl(baseStop);
		armControl(127);
		wait1Msec(1000);
		clawControl(-127);
		wait1Msec(500);
		clawControl(0);
		armControl(-127);
		wait1Msec(1500);
		armControl(0);
}

void backRowAuton (bool side)
{
		deploy();
		driveControl(maxForward);
		clawControl(-127);
		wait1Msec(singleTileTime);
		clawControl(0);
		wait1Msec(singleTileTime);
		driveControl(baseStop);
		clawControl(127);
		wait1Msec(500);
		driveControl(maxBackward);
		wait1Msec(singleTileTime*2);
		if (side)
		{
				driveControl(0,-127);
		}
		else
		{
			driveControl(-127,0);
		}//
		wait1Msec(500);
		driveControl(maxBackward);
		wait1Msec(singleTileTime*2);
		driveControl(baseStop);
		armControl(127);
		wait1Msec(1000);
		clawControl(-127);
		wait1Msec(500);
		clawControl(0);
		armControl(-127);
		if(side)
		{
				driveControl(0,127);
		}
		else
		{
				driveControl(127,0);//
		}
		wait1Msec(100);
		driveControl(baseStop);
		wait1Msec(1400);
		armControl(0);
		driveControl(maxForward);
		wait1Msec(singleTileTime*2);
		clawControl(127);
		driveControl(baseStop);
		wait1Msec(500);
		driveControl(maxBackward);
		wait1Msec(singleTileTime*2);
		driveControl(baseStop);
		armControl(127);
		wait1Msec(1000);
		clawControl(-127);
		wait1Msec(500);
		clawControl(0);
		armControl(-127);
		wait1Msec(1500);
		armControl(0);
}

//Divider for readability

void pre_auton()
{

}


//Divider for readability

task autonomous()
{
		int auton = SensorValue[ArmPot]/1024;
		if (auton == 0)
		{
				cubeAuton(true);
		}
		else if (auton == 1)
		{
				cubeAuton(false);
		}
		else if (auton == 2)
		{
				backRowAuton(true);
		}
		else if (auton == 3)
		{
				backRowAuton(false);
		}
}

//Divider for readability

task usercontrol()
{
		startTask(ArmPID);
    while (true)
		{
			driveControl(mapJoyInt(Ch3),mapJoyInt(Ch2));
			//armControl(mapButtonInt6()*127);
			stateArmControl();

			clawControl(mapButtonInt5()*127);

			button6Switch();
		}
}