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)
//set variables for arcade drive
int maxPower = 127;
int minPower = -127;
int deadband = 5;
// 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
task main()
{
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;
}
//ignores values within deadband range
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!