# PID Control Programming Help in RobotC

Hi, I’m a novice programmer and I need some help programming open-loop PID control incorporated with my arcade drive code. I have a functioning arcade drive drive train that works well enough, but I want to take the extra step and implement open-loop PID control. The robot is for a small competition for my robotics class so I do not have encoders or sensors. I do not have access to the robot for a couple weeks but I’d like to have something that will work in theory because I’m on a huge time crunch (cough hardware issues cough). This is what I currently have:

``````#pragma config(Motor,  port2,           leftMotor,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           rightMotor,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           leftLift,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           rightLift,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           centerLift,    tmotorVex393_MC29, openLoop)

int maxPower = 127;
int minPower = -127;

// Set constants for the PID control loop
const float kp = 0.5;  // Proportional gain
const float ki = 0.1;  // Integral gain
const float kd = 0.1;  // Derivative gain

// Set the target speed for the motors
const float targetSpeed = 50;  // Target speed in RPM

// Set variables for the PID control loop
float error = 0;         // Error value
float integral = 0;      // Integral term
float derivative = 0;    // Derivative term
float previousError = 0; // Previous error value

{

while(1 == 1){

//Calculates motor power
float leftPower = (vexRT[Ch2] + vexRT[Ch1]);
float rightPower = (vexRT[Ch2] - vexRT[Ch1]);

// check calculated amount against min/max
if (rightPower > maxPower) {

rightPower = maxPower;
}
else if (rightPower < minPower){

rightPower = minPower;

}
if (leftPower > maxPower) {

leftPower = maxPower;

}
else if (leftPower < minPower) {

leftPower = minPower;

}

if (abs(leftPower) < deadband ) leftPower = 0;

if (abs(rightPower) < deadband ) rightPower = 0;

//sets motor power
motor[leftMotor]  = leftPower;

motor[rightMotor] = rightPower;

//center lift motor up

if(vexRT[Btn6U] == 1) {

motor[centerLift] = 55;

}

//center lift motor down
else if(vexRT[Btn6D] == 1) {

motor[centerLift] = -35;

}

else {

motor[centerLift] = 0;

}

//lift main lift up
if (vexRT[Btn5U] == 1) {

motor[leftLift] = 65;
motor[rightLift] = 65;

}

//drop main lift
else if (vexRT[Btn5D] == 1) {

motor[leftLift] = -40;
motor[rightLift] = -40;

}

//If buttons are not pressed, nothing happens
else{

motor[leftLift] = 0;
motor[rightLift] = 0;

}

}

// Calculate the error between the current speed and the target speed
error = targetSpeed - getMotorVelocity(leftMotor);
error = targetSpeed - getMotorVelocity(rightMotor);

// Update the integral term
integral += error;

// Calculate the derivative term
derivative = error - previousError;

// Calculate the output value for the PID control loop
float output = (kp * error) + (ki * integral) + (kd * derivative);

// Set the motor speeds using the output value
setMotorSpeed(leftMotor, output);
setMotorSpeed(rightMotor, output);

// Update the previous error value
previousError = error;

// Wait for a short time before iterating the loop again
wait1Msec(25);

}
``````

All the non PID code is functional. Everything else compiles but I haven’t tested at all and is likely very incorrect. Any help is greatly appreciated, even anything with the way my code is organized or commented. Thank you!