Need help with PID

VEX V5 General Discussion Technical Discussion
1

I am new to coding so be gentle.

I am having trouble with the mogo lift. I am using a 4 bar with 2 motors with a 1:7 gear ratio for torque. Without a load, the lift works fine. When a load is introduced, it seems to work once, if I am lucky, and then does not work.

I have been told that I may not be getting enough power to the motors with the PID control. Any help with this would be greatly appreciated.

I am posting my code here:

if(vexRT[Btn5U]) {
		pidGoal = SensorValue[frontMogoLeft] + 900;
	}

	else if(vexRT[Btn5D]) {
		pidGoal = SensorValue[frontMogoLeft] - 900;

		//else
			//pidGoal = 0;

	}

Here is the PID task file titled mogoPid.c:

static float pidKp = 0.35;
static float pidGoal;
float pidGoalR;

/-----------------------------------------------------------------------------/
/* pid control task /
/-----------------------------------------------------------------------------*/

task pidMogoL()
{
float pidSensorCurrentValue;
float pidError;
float pidDrive;

while( true )
    {
    // Read the sensor value and scale
    pidSensorCurrentValue = SensorValue[frontMogoLeft];

    // calculate error
    pidError =  pidGoal - pidSensorCurrentValue;

    // calculate drive
    pidDrive = (pidKp * pidError);

    // limit drive
    if( pidDrive > 127 )
        pidDrive = 127;
    if( pidDrive < (-127) )
        pidDrive = (-127);

    // send to motor
    motor[frnMogoLeft] = pidDrive;
    pidGoalR = pidGoal + 52;

    // Don't hog cpu
    wait1Msec( 25 );
    }

}

task pidMogoR()
{
float pidSensorCurrentValue;
float pidError;
float pidDrive;

while( true )
    {
    // Read the sensor value and scale
    pidSensorCurrentValue = SensorValue[frontMogoRight];

    // calculate error
    pidError =  pidGoalR - pidSensorCurrentValue;

    // calculate drive
    pidDrive = (pidKp * pidError);

    // limit drive
    if( pidDrive > 127 )
        pidDrive = 127;
    if( pidDrive < (-127) )
        pidDrive = (-127);

    // send to motor
    motor[frnMogoRt] = pidDrive;

    // Don't hog cpu
    wait1Msec( 25 );
    }

}

I know I have probably done something incredibly stupid and made a very simple oversight of something very basic but with no coding experience, I really am lost. Any help would be greatly appreciate.

I am confident the mechanics are sound so I must be doing something wrong in my code.

2

your code looks pretty solid to me, but you could try giving your motors a constant power. In a PID loop, this is the I. What you have now is the P which means as the mogo gets closer to the height you want, it gets less power. It is therefore basically impossible to get the mogo to the height you want and keep it there using just the P. One of Two easy solutions is taking a running sum of all the error and adding that times a constant to your power. This is the “I” part of PID representing the integral. The second solution is something I’ve found success in is adding the current motor power to the power that the motor gets.


motor[frnMogoRt]=pidDrive+motor[frnMogoRt];

and


motor[frnMogoLeft]=pidDrive+motor[frnMogoLeft];

you will have to retune your constants either way

3