Hi!
So I’ve written a task to run PID for my drive. (Let me know if you’d like the whole code, at the moment I am just going to c/p the stuff that’s actually necessary to understand what’s going on). I have not tested this, and I need some fresh eyes to take a look at it and tell me what may potentially go wrong, some tips on starter constants, and potentially whole different ways of turn error correction.
Thanks a lot, guys.
DriveR and DriveL are for the two respective sides of the drive.
///////////////////////////////////
//Robot-based Definitions//
//////////////////////////////////
#define DRIVE_TYPE "dropcenter"
#define TPR 360
#define WHEEL_SIZE 5
#define WHEEL_CIRC 15.70
#define LCD_LEFT 1
#define LCD_CENTER 2
#define LCD_RIGHT 4
////////////////////////////////////////////////////////////////////////
//DRIVE PID, BASIC CONVERSION AND INIT FUNCTIONS//
///////////////////////////////////////////////////////////////////////
void driveR(int power)
{
motor(DriveLL) = motor(DriveLR) = (val > 127) ? 127 * sgn(power) : power;
}
void driveL(int power)
{
motor(DriveRL) = motor(DriveRR) = (val > 127) ? 127 * sgn(power) : power;
}
void gyroInit()
{
SensorType[in1] = sensorNone;
wait1Msec(1000);
SensorType(in1) = sensorGyro;
wait1Msec(2000);
}
float inchTicks(float inch) {
float ticks = TPR * (inch / WHEEL_CIRC);
return ticks;
}
float ticksInch(float sensor) {
return (sensor / TPR) * WHEEL_CIRC;
}
float target = 0;
int maxDrivePower = 127;
task fwdDrivePID()
{
float Kp = 0.3;
float Ki = 0.5;
float Kd = 1.5;
float proportional = 0;
float derivative = 0;
float integral = 0;
float error = 1;
float prevError = 0;
float iLimit = 100;
long lEncoder, rEncoder;
int speed;
float dT = time1(T1) - t;
float turnError;
float turnPower;
long turnTarget = 0;
float turnKp = 0.34;
float turnRatio = 7;
while (speed > 0) { // Is saying speed > 0 a good way of letting the loop escape once the robot's done so I can start another instance of this task?
lEncoder = SensorValue(encLeft);
rEncoder = SensorValue(encRight);
error = target - ((lEncoder + rEncoder) / 2); // error = target - avg of both encoders
turnError = turnTarget - (rEncoder - lEncoder);
proportional = error;
integral = (abs(error) <= 0.25 * target) ? integral + (error*dT) : 0; // only use integral if the error is less than 25% of the target (more simply if the robot is 75% of the way through driving)
integral = (integral >= iLimit) ? 0 : integral; // limit the integral to 100 and reset it if it gets to 100.
derivative = (error - prevError) / dT;
turnPower = ((turnKp * turnRatio) * turnError);
speed = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
if (abs(speed) > 127) {
speed = sgn(speed) * maxDrivePower;
}
DriveR(speed + turnPower); //How might I account for if both speeds are at 127 and can't be changed anymore to account for turning?
DriveL(speed - turnPower);
prevError = error;
wait1Msec(10);
t += dT;
/// if (error == 0) {
/// driveSet(0, 0);
/// lEncoder = 0;
/// rEncoder = 0;
/// return;
/// }
}
}
void fwdDriveInch(float inch) {
stopTask(fwdDrivePID);
target = inchTicks(inch);
startTask(fwdDrivePID);
}
P.S. If you’re willing to give me a hand but need me to tell you what’s going on because of my lack of comments, please let me know.