robot2.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  armEncoder,     sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           rightFrontMotor, tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           leftFrontMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           rightBackMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           rightArmMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           leftIntakeMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           rightIntakeMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           clawLiftMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           leftArmMotor,  tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port9,           clawMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          leftBackMotor, tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

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

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

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
// PID CONSTANTS
float pid_Kp = 0.5;
float pid_Ki = 0.01;
float pid_Kd = 0.06;


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.
	bStopTasksBetweenModes = true;
	nMotorEncoder[leftArmMotor] = 0;
	// 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.
	// .....................................................................................
    motor[leftBackMotor] = -100;
    motor[rightBackMotor] = 100;
    motor[leftFrontMotor] = -100;
    motor[rightFrontMotor] = 100;
    motor[leftArmMotor] = 80;
    motor[rightArmMotor] = -80;
    wait1Msec(1500);
    int tgtPos = nMotorEncoder[leftArmMotor];
    int err = 0;
    int prevErr = 0;
    int err_diff = 0;
    int err_sum = 0;
	int timer = 80;
    while (timer > 0) {
        err = nMotorEncoder[leftArmMotor] - tgtPos;
        err_sum += err;
        err_diff = err - prevErr;
        motor[leftArmMotor] = (err_sum * pid_Ki) + (err * pid_Kp) + (err_diff * pid_Kd);
        prevErr = err;
		timer -= 1;
		wait1Msec(25);
    }
    motor[leftBackMotor] = 0;
    motor[rightBackMotor] = 0;
    motor[leftFrontMotor] = 0;
    motor[rightFrontMotor] = 0;
    motor[leftIntakeMotor] = 127;
    motor[rightIntakeMotor] = -127;
	timer = 80;
    while (true) {
        err = nMotorEncoder[leftArmMotor] - tgtPos;
        err_sum += err;
        err_diff = err - prevErr;
        motor[leftArmMotor] = (err_sum * pid_Ki) + (err * pid_Kp) + (err_diff * pid_Kd);
        prevErr = err;
		timer -= 1;
		wait1Msec(25);
    }
	stopAllMotors();
}

// Globvars
int armSpeed = 80;
bool clawAuto = false;
float clawRatio = 9.0 / 8.0;

// PID Values
int targetPos = 0;
bool offRun = true;
int error;
int prevError;
int error_diff;
int error_sum;

void manualArm()
{
	if(vexRT[Btn5U])
	{
		offRun = true;
		targetPos = nMotorEncoder[leftArmMotor];
		setMotor(leftArmMotor, armSpeed);
	}
	else if(vexRT[Btn5D])
	{
		offRun = true;
		targetPos = nMotorEncoder[leftArmMotor];
		setMotor(leftArmMotor, -armSpeed);
	}
	else
	{
		if (offRun) {
			offRun = false;
			error_sum = 0;
			prevError = 0;
			setMotor(leftArmMotor, 0);
		}
		error = nMotorEncoder[leftArmMotor] - targetPos;
		error_sum += error;
		error_diff = error - prevError;
		motor[leftArmMotor] = (error_sum * pid_Ki) + (error * pid_Kp) + (error_diff * pid_Kd);
		prevError = error;
	}
	motor[rightArmMotor] = -motor[leftArmMotor];
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 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()
{
	while (true)
	{
		int fd = vexRT[Ch3]; // Forward Joystick
		int sd = vexRT[Ch4]; // Side Joystick
		int tr = vexRT[Ch1]; // Turn Joystick
		int lfv = fd + sd + tr;  // Left front motor
		int rfv = -fd + sd + tr; // Right front motor
		int lbv = fd - sd + tr;  // Left back motor
		int rbv = -fd - sd + tr; // Right back motor

		motor[leftFrontMotor] = -lfv;
		motor[rightFrontMotor] = -rfv;
		motor[rightBackMotor] = -rbv;
		motor[leftBackMotor] = -lbv;
		// Intake
		armControl(leftIntakeMotor, Btn7D, Btn7U, -127);
		armControl(rightIntakeMotor, Btn7D, Btn7U, 127);
		// Claw
		armControl(clawMotor, Btn6D, Btn6U, 60);
		// Arm
		if (vexRT[Btn7L]) armSpeed = 80;
		if (vexRT[Btn7R]) armSpeed = 40;
		manualArm();
		if (vexRT[Btn8L]) clawAuto = true;
		if (vexRT[Btn8R]) clawAuto = false;
		if (clawAuto) {
			armControl(clawLiftMotor, Btn5U, Btn5D, armSpeed * clawRatio);
		}
		else {
			armControl(clawLiftMotor, Btn8U, Btn8D, 45);
		}
		wait1Msec(25);
	}
}