PID loop help

VEX V5 Technical Support
#1

I’m using 2 IME’s, one on each side of the drivetrain. I made a PID function to move forward, back, turn right and left. But I’m not sure if the code is right. I haven’t tuned the Kp, Ki, and Kd values yet, so don’t worry about that.


#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,            ,             tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,            ,             tmotorVex393_MC29, PIDControl, encoderPort, I2C_1)//right
#pragma config(Motor,  port5,            ,             tmotorVex393_MC29, PIDControl, encoderPort, I2C_2)//left
#pragma config(Motor,  port6,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,            ,             tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,           ,             tmotorVex393_HBridge, openLoop, reversed)

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

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

float Kp = 0.01;
float Ki = 0;
float Kd = 0;

void moveForward(float distance)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left



slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = 57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//convert distance in inches to raw values
errorLeft = distance * 57.2967795;
errorRight = distance * 57.2967795;

//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise motors will keep running
if(errorLeft < integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight < integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole numbers
round(speedLeft);
round(speedRight);

//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}


void moveBackward(float distance)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//convert distance in inches to raw values
errorLeft = distance * -57.2967795;
errorRight = distance * -57.2967795;

//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//hard stop/limit for integral, otherwise integral will keep motors running
if(errorLeft > integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight > integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}

void turnRight(float degrees)
{
	slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//right side back, left side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * 4582.131913;
errorRight = (degrees/360) * -4582.131913;


//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise motors will keep running
if(errorLeft < integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight > integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}


void turnLeft(float degrees)
{
	slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//left side back, right side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * -4582.131913;
errorRight = (degrees/360) * 4582.131913;


//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise it will keep motors running
if(errorLeft > integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

//add hard stop/limit to integral otherwise motors will keep running
if(errorRight < integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}


void pre_auton()
{
  bStopTasksBetweenModes = true;
  //reset shaft encoders
nMotorEncoder[I2C_1] = 0;
nMotorEncoder[I2C_2] = 0;
task autonomous()
{
moveForward(20);
moveBackward(13);
turnRight(90);
}
#2

The way you have it right now, the base will move forward for 25ms, reverse for 25ms, and turn right for 25ms. The equations is good, but you need to put that into a while loop to calculate all the values.
Example:


void MoveForward(int Target)
{
  while(true)
  {
    "PID calculations"
    motor[leftside]=PID_Output_Power;
    motor[RightSide]=PID_Output_Power;
    wait1Msec(20);//Max refresh rate for ports 2-9
  }
}

I recomend this doc for learning pid:link text

#3

To add on to @Nandobob23 's solution, I would recommend to reset your motor encoders before each while loop.

When the motor encoders are reset before each while loop, the


distance

parameter will act as a relative position (ex: go forward 20 inches, then from your current position go back 13 inches).

Appended example:

void MoveForward(int Target)
{
//reset encoders
nMotorEncoder[I2C_1] = 0;
nMotorEncoder[I2C_2] = 0;
  while(true)
  {
    "PID calculations"
    motor[leftside]=PID_Output_Power;
    motor[RightSide]=PID_Output_Power;
    wait1Msec(20);//Max refresh rate for ports 2-9
  }
}
#4

Thanks! is there anything else that I should fix?

#5

here is my fixed code:

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,            ,             tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,            ,             tmotorVex393_MC29, PIDControl, encoderPort, I2C_1)//right
#pragma config(Motor,  port5,            ,             tmotorVex393_MC29, PIDControl, encoderPort, I2C_2)//left
#pragma config(Motor,  port6,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,            ,             tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,           ,             tmotorVex393_HBridge, openLoop, reversed)

float Kp = 0.01;
float Ki = 0;
float Kd = 0;

void moveForward(float distance, float maxSpeed)
{
	nMotorEncoder[I2C_1] = 0;
	nMotorEncoder[I2C_2] = 0;
while(true)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = 57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//convert distance in inches to raw values
errorLeft = distance * 57.2967795;
errorRight = distance * 57.2967795;

//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise motors will keep running
if(errorLeft < integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight < integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole numbers
round(speedLeft);
round(speedRight);

if(speedLeft > maxSpeed)
{
speedLeft = maxSpeed;
}
if(speedRight > maxSpeed)
{
speedRight = maxSpeed;
}
//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}
}

void moveBackward(float distance, float maxSpeed)
{
	nMotorEncoder[I2C_1] = 0;
	nMotorEncoder[I2C_2] = 0;
while(true)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//convert distance in inches to raw values
errorLeft = distance * -57.2967795;
errorRight = distance * -57.2967795;

//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//hard stop/limit for integral, otherwise integral will keep motors running
if(errorLeft > integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight > integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

if(speedLeft < (maxSpeed * -1))
{
speedLeft = maxSpeed;
}
if(speedRight < (maxSpeed * -1))
{
speedRight = maxSpeed;
}
//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}
}
void turnRight(float degrees, float maxSpeed)
{
	nMotorEncoder[I2C_1] = 0;
	nMotorEncoder[I2C_2] = 0;
while(true)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//right side back, left side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * 4582.131913;
errorRight = (degrees/360) * -4582.131913;


//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise motors will keep running
if(errorLeft < integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

if(errorRight > integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

if(speedLeft < (maxSpeed * -1))
{
speedLeft = maxSpeed;
}
if(speedRight > maxSpeed)
{
speedRight = maxSpeed;
}
//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}
}

void turnLeft(float degrees, float maxSpeed)
{
	nMotorEncoder[I2C_1] = 0;
	nMotorEncoder[I2C_1] = 0;
while(true)
{
slaveMotor(port7,port5);
slaveMotor(port5,port3);//left

slaveMotor(port6,port4);
slaveMotor(port4,port1); //right

slaveMotor(port10, port2); //flywheel

float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;

//left side back, right side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * -4582.131913;
errorRight = (degrees/360) * 4582.131913;


//calculates new error based on distance left from objective
errorLeft = errorLeft - nMotorEncoder[I2C_1];
errorRight = errorRight - nMotorEncoder[I2C_2];

//add hard stop/limit to integral, otherwise it will keep motors running
if(errorLeft > integralActiveZone && errorLeft != 0)
{
	errorTotalLeft += errorLeft;
}
else
{
	errorTotalLeft = 0;
}

//add hard stop/limit to integral otherwise motors will keep running
if(errorRight < integralActiveZone && errorRight != 0)
{
	errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}

//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;

//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;

//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;

//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);

//round speeds to whole number
round(speedLeft);
round(speedRight);

if(speedLeft > (maxSpeed * -1))
{
speedLeft = maxSpeed;
}
if(speedRight < maxSpeed)
{
speedRight = maxSpeed;
}
//motors move at speed found
motor(port3) = speedLeft;
motor(port1) = speedRight;

//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;

wait1Msec(25); //let motors and cortex catch up
}
}

void pre_auton()
{
  bStopTasksBetweenModes = true;
  //reset shaft encoders
nMotorEncoder[I2C_1] = 0;
nMotorEncoder[I2C_2] = 0;
}

task autonomous()
{
moveForward(20, 127);
moveBackward(13, 127);
turnRight(90, 90);
moveForward(20, 127);
moveBackward(20, 127);
turnLeft(90, 127);

}
#6

@mkinh, first of all it looks like your motor port assignments are:

3,5,7 for the left side,
1,4,6 for the right side, and
2,10 for the flywheel.

Ports 1 and 10 are special in the sense that they use Cortex built-in H-Bridges that have slightly different characteristics than MC29 motor controllers that are plugged into ports 2-9.

I would instead use matched ports 1 and 10 for the flywheel, and put drive motors on {3,5,7} and {2,4,6} groups.

Second, you definitely want to check out another recent PID thread, where an example of the code was posted that keeps separate PD loop for driving forward and PI loop for keeping the robot heading straight. That method is preferable to the keeping two separate PID loop for the left and right side of the drivetrain.

Your code has very nice handling of the integral component as you get close to the target (i.e. integralActiveZone) and if you could add this to the PD loop posted in another thread you could get yourself very compact and capable generic PID code that could get you very good autonomous performance.

#7

I would recommend replacing the condition in the while loop with something like


abs(errorLeft) > threshold && abs(errorRight) > threshold

, where


threshold

is the minimum acceptable distance (in encoder ticks) the robot can be from the target distance. This will make the PID controller run until the robot is close enough to the target, at which point the while loop will exit.

A condition like


abs(errorLeft) > 0 && abs(errorRight) > 0

should also work similarly, except the while loop will exit once the robot reaches the actual target.

I would also recommend to declare these variables before the while loop begins:


float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;

Variables such as


errorTotalLeft

,


errorTotalRight

,


lastErrorLeft

, and


lastErrorRight

rely on what happened in previous iterations of the while loop. For example if left declared inside of the while loop, then


lastErrorLeft

and


lastErrorRight

wouldn’t retain the previous error value, and


errorTotalLeft

and


errorTotalRight

wouldn’t accumulate error. By declaring those variables outside of the while loop, they should be able to function as intended.

One last thing I would add, although doesn’t directly affect your PID’s performance, is to turn off the drivetrain motors after the while loop ends. Though the drivetrain would be moving slowly after it exits the while loop, setting the motors to zero after the while loop will ensure the robot actually stops at/around the target.

Modified example:

void MoveForward(int Target)
{
//reset encoders
nMotorEncoder[I2C_1] = 0;
nMotorEncoder[I2C_2] = 0;

//declare function-global variables
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight

  while(abs(errorLeft) > threshold && abs(errorRight) > threshold)
  {
    //PID math here
    motor[leftside]=PID_Output_Power;
    motor[RightSide]=PID_Output_Power;
    wait1Msec(20);//Max refresh rate for ports 2-9
  }
//turn off motors
motor[leftside]= 0;
motor[RightSide]= 0;
}
#8

Thanks!

#9

we have a 4 motor flywheel now, so we can’t use ports 1 and 10 only.

#10

@mkinh,

Turning Point’s balls have almost the same weight as the NbN balls and it was possible to build a flywheel powered by a single 393 motor even though you had to launch a lot of balls one after another. The trick was to align everything well to minimize friction.

We also found that having two motors on ports 1 and 10 would give you the best accuracy, because those ports react very quickly to the power change commands.

And with 3 motors you could launch the balls almost as fast as two humans could load them into a robot. However, when we added one more motor for the total of 4, it degraded both accuracy and speed, because it was way harder to tune PID to be stable.

This season you don’t need to send a lot of balls in the rapid sequence. You just need short bursts of one or two balls (if you have double-shot) and then you could take time to spin-up the flywheel while you hunt for more balls. The best strategy for Turning Point would be to make flywheel slightly heavier to store enough energy for launching two balls. Many teams add extra wheels on the flywheel axle to increase its moment of inertia.

If you still have time to do physical modifications, I would recommend you to investigate reducing the friction, increasing flywheel’s moment of inertia, and try to get it working with just two motors.

However, if you need to make it work with 4 motors, then the easiest thing would be to connect all motors through MC29 controllers and use ports 1 and 10 for other functions, like intake and indexing. It is best not to mix port types within one subsystem.

If you really, really, need to mix and match the ports then you need to send different power levels to the different ports. This a diagram of the power level that you need to send to the motor ports (y-axis) depending on the desired motor RPM (x-axis):

More details available in this post: Trial And Error PID

Cortex V4 6 motor drive problems
#11