973G 2017.c

#pragma config(Sensor, in1,    armPot,         sensorPotentiometer)
#pragma config(Sensor, in2,    clawPot,        sensorPotentiometer)
#pragma config(Sensor, dgtl7,  FLEncoder,      sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  FREncoder,      sensorQuadEncoder)
#pragma config(Motor,  port2,           frontRight,    tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           frontLeft,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           LRLiftM,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           LLiftM,        tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port6,           RLiftM,        tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port7,           ClawM,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           backRight,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           backLeft,      tmotorVex393TurboSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)//3500, 4095
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#define yAxis vexRT[Ch3]
#define xAxis vexRT[Ch1]
#define rotation vexRT[Ch1]

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

float clawTarget = SensorValue[clawPot]; //base value need to change in testing
float clawGain = 0.2;
int clawOutput = 0;
bool isHanging = false;

int deadband(int joystickTarget) {
	if (abs(joystickTarget) <= 20) {
		joystickTarget = 0;
	}
	return joystickTarget;
}

void liftMotorSet(int value) {
	motor[LLiftM] = deadband(value);//	#8
	motor[RLiftM] = deadband(value);// #9
	motor[LRLiftM] = deadband(value);// #7
}

task clawMotorControl(){
 while(true) {
		motor[ClawM] = clawOutput;
		if (vexRT[Btn6D] == true) { // star clamp
			clawTarget = 1450;
		}
		else if (vexRT[Btn6U] == true) { //cube prep
			clawTarget = 2100;
		}
		else if (vexRT[Btn8D] == true) { //outWide
			clawTarget = 2715;
		}
		else if(vexRT[Btn8DXmtr2] == true) {
			clawTarget = 3500;
		}
		else if(vexRT[Btn8UXmtr2] == true) {
		 clawTarget = 4095;
		}
		if(abs(clawTarget - SensorValue[clawPot]) > 10){
			clawOutput = clawGain * (clawTarget - SensorValue[clawPot]);
		}
		else {
			clawOutput = 0;
		}
	}
}

task noPidArm() {
	while(isHanging == false){
		liftMotorSet(deadband(vexRT[Ch2Xmtr2]));
		if(SensorValue[armPot] > 1900) {
			clawTarget = 2715;
		}
	}
}

task Hang() {
	while(vexRT[Btn8RXmtr2] == true) {
		isHanging = true;
		clawTarget = 3700;
		liftMotorSet(127);
		if(SensorValue[armPot] < 500) {
			clawTarget = 4095;
			wait1Msec(1000);
			liftMotorSet(0);
		}
	}
	isHanging = false;
}
void autoDrive(int xValue, int yValue, int rValue){
	motor[frontLeft] = +xValue + yValue - rValue;//Black-Black
	motor[frontRight] = -xValue + yValue + rValue;//??
	motor[backLeft] = +xValue - yValue + rValue;//Black-Black
	motor[backRight] = -xValue - yValue- rValue;//Black-Red
}

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
}

task autonomous()
{
	autoDrive(0,-110,0);
	wait1Msec(50);
	autoDrive(0,0,0);
}

task usercontrol(){
	while (true) {
		startTask (Hang);
		startTask (noPidArm);
		startTask (clawMotorControl);
		liftMotorSet(vexRT[Ch2Xmtr2]);

		if (vexRT[Btn5U]) {
			autoDrive(xAxis, 0, rotation);
		}
		else {
			autoDrive(xAxis, yAxis, 0);
		}
	}
}