# Arm PID

Hello everyone,

I was trying to program a PID for my dumper arms. They do not travel at exactly the same speed, so I created a PID to solve this. However, I think my logic behind the program is flawed.

``````
{
// User control code here, inside the loop

while (true)
{
int targetVelocity = 130;
float dumpErrorL = targetVelocity - getMotorVelocity(LeftCatapult2);
float dumpErrorR = targetVelocity - getMotorVelocity(RightCatapult2);
float kp = .7;
int dumpSpeedL = dumpErrorL * kp;
int dumpSpeedR = dumpErrorR * kp;

if(vexRT[Btn5UXmtr2] == 1) {

motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = dumpSpeedR;
}
else if(vexRT[Btn5DXmtr2] == 1) {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = -1 * dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = -1 * dumpSpeedR;
}
else {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = 0;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = 0;
}
}
}

``````

The issue is that the when the velocity of the motors reaches the target, the error is 0 which also makes the dumpSpeed 0. It needs to stop altering the motor speed once they reach the correct velocity. How can I do this? I remember that a lot of people had this same issue with their flywheels last year since the logic is the same but I couldn’t find a solution to this.

So what you are currently using is a P loop, this means that the speed is proportional to the error which in some situations (think with a thermostat and heater) might be optimal. To remedy this we have two possible solutions, an integral approach (a true Proportional, Integral and Derivative loop) or a K value. A k value is a constant value around where you might want your arm to be moving when you press a button. This means that your P loop works around this value to achieve the same speed over the two arms. The second (and IMO better approach) is to use an actual PID loop (with specific notice of the integral term). The integral term is the buildup of error over time and means that as the proportional lowers the integral increases and then plateaus (with some assistance from integral control constants) causing your arm to actually run at the specified speed.

Ok, so I looked at it and this is what I have so far.

``````
void dumperStraightening() {
while(vexRT[Btn5UXmtr2] == 1 || vexRT[Btn5DXmtr2] == 1) {
velocityL = getMotorVelocity(LeftCatapult2);
velocityR = getMotorVelocity(RightCatapult2);

dumpErrorL = targetVelocity - velocityL;
dumpErrorR = targetVelocity - velocityR;

if(dumpErrorL<integralActiveZone && dumpErrorL != 0) {
dumpErrorTL += dumpErrorL;
}
else {
dumpErrorTL = 0;
}

if(dumpErrorR<integralActiveZone && dumpErrorR != 0) {
dumpErrorTR += dumpErrorR;
}
else {
dumpErrorTR = 0;
}
if(dumpErrorTL > 50/Ki) {
dumpErrorTL = 50/Ki;
}
if(dumpErrorTR > 50/Ki) {
dumpErrorTR = 50/Ki;
}
if(dumpErrorL == 0) {
derivitiveL = 0;
}
if(dumpErrorR == 0) {
derivitiveR = 0;
}
proportionL = dumpErrorL * Kp;
proportionR = dumpErrorR * Kp;
integralL = dumpErrorTL * Ki;
integralR = dumpErrorTR * Ki;
derivitiveL = (dumpErrorL - lastErrorL) * Kd;
derivitiveR = (dumpErrorR - lastErrorR) * Kd;

lastErrorL = dumpErrorL;
lastErrorR = dumpErrorR;

dumpspeedL = proportionL + derivitiveL + integralL;
dumpspeedR = proportionR + derivitiveR + integralR;

//shooterspeedL = proportionL + derivitiveL;
//shooterspeedR = proportionR + derivitiveR;

motor[LeftCatapult2] = motor[LeftCatapult1] = motor[LeftBottomCatapult] = dumpSpeedL;
motor[RightCatapult2] = motor[RightCatapult1] = motor[RightBottomCatapult] = dumpspeedR;
controlRobot();
}

}

void controlRobot() {
if (vexRT[Btn5UXmtr2] == 1 || vexRT[Btn5DXmtr2] == 1)
{

motor[RightFront] = motor[RightBack] = vexRT[Ch2];
motor[LeftFront] = motor[LeftBack] = vexRT[Ch3];

if(vexRT[Btn5U] == 1) {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = 127;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = 127;
}
else if(vexRT[Btn5D] == 1) {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = -127;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = -127;
}

else if(vexRT[Btn5UXmtr2] == 1) {
targetVelocity = 130;
dumperStraightening();
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = dumpSpeedR;
}
else if(vexRT[Btn5DXmtr2] == 1) {
dumperStraightening();
targetVelocity = -130;
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = -1 * dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = -1 * dumpSpeedR;
}
else {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = 0;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = 0;
}
}
}

//PID Variables------------------------------------------------------------------------------------------//

void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks
// running between Autonomous and Driver controlled modes. You will need to
// manage all user created tasks if set to false.

// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
// used by the competition include file, for example, you might want
// to display your team name on the LCD in this function.
// bDisplayCompetitionStatusOnLcd = false;

// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}

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

{
// ..........................................................................
// Insert user code here.
// ..........................................................................

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

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

{
// User control code here, inside the loop

while (true)
{

motor[RightFront] = motor[RightBack] = vexRT[Ch2];
motor[LeftFront] = motor[LeftBack] = vexRT[Ch3];

if(vexRT[Btn5U] == 1) {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = 127;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = 127;
}
else if(vexRT[Btn5D] == 1) {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = -127;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = -127;
}

else if(vexRT[Btn5UXmtr2] == 1) {
targetVelocity = 130;
dumperStraightening();
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = dumpSpeedR;
}
else if(vexRT[Btn5DXmtr2] == 1) {
dumperStraightening();
targetVelocity = -130;
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = -1 * dumpSpeedL;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = -1 * dumpSpeedR;
}
else {
motor[LeftBottomCatapult] = motor[LeftCatapult1] = motor[LeftCatapult2] = 0;
motor[RightBottomCatapult] = motor[RightCatapult1] = motor[RightCatapult2] = 0;
}
}

``````

With this PID, the arm does not even move. I created the controlRobot function so that the rest of the robot works while the robot is in the loop for the dumperStraightening function.

Okay, so I don’t have a copy of RobotC with me, so you will have to do some debugging. But the extra functions and statements seemed a little frivolous to me so I put together two tasks with the code I copied and pasted from above. In order to make a controller that will try and meet the set target velocity. You then set the speed you want both your dumpers to be moving at in the main control task and the PID will do its best to set both the arms to that speed.

``````const long Kp = 0.7;    // We use constant values as they are better when you compile
const long Ki = 0.1;    // K denotes a constant.
const long Kd = 0.1;

// Global value for the targetVelocity to send to the task
long targetVelocity = 0;    // This will be the inital speed of the dumper

// This will be the task that changes the speed of the
// dumper based on what you set the targetVelocity to
while( true ) {
// Get the error
float dumpErrorL = targetVelocity - getMotorVelocity(LeftCatapult2);
float dumpErrorR = targetVelocity - getMotorVelocity(RightCatapult2);

if(dumpErrorL<integralActiveZone && dumpErrorL != 0) {
dumpErrorTL += dumpErrorL;
}
else {
dumpErrorTL = 0;
}
if(dumpErrorR<integralActiveZone && dumpErrorR != 0) {
dumpErrorTR += dumpErrorR;
}
else {
dumpErrorTR = 0;
}

// Restrict Ki
if(dumpErrorTL > 50/Ki) {
dumpErrorTL = 50/Ki;
}
if(dumpErrorTR > 50/Ki) {
dumpErrorTR = 50/Ki;
}

// Restrict Kd
if(dumpErrorL == 0) {
derivitiveL = 0;
}
if(dumpErrorR == 0) {
derivitiveR = 0;
}

// Calculate the PID
proportionL = dumpErrorL * Kp;
proportionR = dumpErrorR * Kp;
integralL = dumpErrorTL * Ki;
integralR = dumpErrorTR * Ki;
derivitiveL = (dumpErrorL - lastErrorL) * Kd;
derivitiveR = (dumpErrorR - lastErrorR) * Kd;

lastErrorL = dumpErrorL;
lastErrorR = dumpErrorR;

dumpspeedL = proportionL + derivitiveL + integralL;
dumpspeedR = proportionR + derivitiveR + integralR;

// Set the values
motor[LeftCatapult2] = motor[LeftCatapult1] = motor[LeftBottomCatapult] = dumpSpeedL;
motor[RightCatapult2] = motor[RightCatapult1] = motor[RightBottomCatapult] = dumpspeedR;

wait1Msec( 25 );
}
}

{
// We start the repeating code that controls the dumper

while (true)
{
// We set the speed we want to travel at to
// 130
if(vexRT[Btn5UXmtr2] == 1) {
targetVelocity = 130;
}
// We set the speed we want to travel at
// to -130
else if(vexRT[Btn5DXmtr2] == 1) {
targetVelocity = -130;
}
// We set the speed we want to travel at to 0
else {
targetVelocity = 0;
}
wait1Msec( 25 );
}
}
``````

Sorry, i’m in a bit of a hurry with this post so sorry if I missed something!

Thank you soooooooo much for your help. I have one question though. If the task starts inside user control and it enters a loop, then won’t the rest of the robot not be able to function? Or does the usercontrol task run at the same time as the dumperControl task?

A task runs in parallel with the other tasks getting little bits of compute time. In while loops in each task, put a wait in there for a few milliseconds to help allow nice usage of the tasks to context switch between each other.

Yeah, so once one of the tasks has finished functioning it will let the other task run and so on. Take a look at this article for an explanation as tasks can be really helpful at times :).