robot.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.
//
/////////////////////////////////////////////////////////////////////////////////////////

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;

	// 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.
  // .....................................................................................

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

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 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()
{
	// PID CONSTANTS
	float pid_Kp = 0.5;
	float pid_Ki = 0.04;
	float pid_Kd = 0.04;

	// PID Values
	int targetPos = 0;
	bool offRun = true;
	nMotorEncoder[leftArmMotor] = 0;
	int error;
	int prevError;
	int error_diff;
	int error_sum;
	

	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
		int armSpeed = 80;
		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 = targetPos - nMotorEncoder[leftArmMotor];
			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];
		armControl(clawLiftMotor, Btn8U, Btn8D, 90);
		wait1Msec(25);
	}
}

@Genora51
add

void slaveMaster(tMotor slave, tMotor master, float KP, float ratio){
    float error = (nMotorEncoder[master] * ratio) - nMotorEncoder[slave];
    motor[slave] = error * KP;
}

to the top of the file then make line 125

slaveMaster(clawLiftMotor, whaterverArmMotorHasTheIme, someKPvalue, twoOverThreeOrThreeOverTwo);

but replacing the values Obviously

just P control but its enough

Wouldn't that mean that if it reached the target, it would stop even if the button was still held? Is there instead a way to make sure that corrections are relative to the initial speed?

Also, you'll need to change nMotorEncoder(...) to nMotorEncoder[...] because for some reason it's an array or something.

changes to nMotorEncoder[...]. will look up if this will work