P & PID loop coding help ROBOTC mainly

#1

Welcome to this chat mainly for programmers who need help with p or pid

1 Like

#2

P, or proportional, is known as error, is sensorvalue - target
I is Integral, generally known as totalError, which is calculated by += error
D is the derivative which is error - previousError

kP is a multiplier to tune error (P)
kI is a multiplier to tune totalError(I)
kD is a multiplier to tune derivative(D)
kP, kI, and kD MUST be floats or doubles(but doubles are not supported in ROBOTC) because they will be most likely within decimal places, and potentially down to the 100ths place.

To add them all up, you would do:
MotorPower = (error * kP + totalError * kI + derivative * kD);

P is the most bare-bones simple you can have, and the I and D can be added/removed into the control loop depending if they are required or not. The D controls the speed and generally will add/remove power and compensate the slight battery voltage changes in the motor and/or friction, and the I helps do slight movements to help correct the arm/mechanism/speed to the desired value in the closer margins of the desired value. Let me write an example (Btw this code is barebones simple and doesn’t follow some of the better habits, but since it’s getting pretty late here I wrote what I can) :

task usercontrol(){
int target = 0;
int previousError = 0;

//Tune here
double kP = 0.054;
double kI = 0.043;
double kD = 0.42;

while(1==1){

if(vexRT[Btn6U] == 1){
target = 300;
}
else{
target = 0;
}

int error = SensorValue[Ecoder] - target;
int totalError += error;
int derivative = error - previousError;

int motorPower = (error * kP + totalError * kI + derivative * kD); //Add values up
motor[Wrist] = motorPower; //Apply values to motor

//Since code is read from top to bottom, this will be read, 
//then 20 miliseconds will pass before the next loop causing this value
//To become a value 20 miliseconds ago
previousError = error; 
wait1msec(20);
}

}

If you want to learn how to tune a PID, you can check this website here: https://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops

5 Likes

#3

WOAH (Sorry to be off topic) VEXForum just made a cool symbol
Apparently § is caused by doing ( P) (without spaces in between)
I never knew that would happen… That’s very interesting.

0 Likes

Staying Straight while going forward and motorCoast not working
#4

yes, I fixed that for you.
I was going to remove this topic as it didn’t really ask a question and was flagged, however, as @Connor1814D wrote such a comprehensive explanation, I’ll leave it alone

you can escape markdown special characters by using \
so (P) is written as \(P\)

4 Likes

#5

#pragma config(Motor, port2, motorintake, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, motorleft, tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor, port4, motorleftbottom, tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor, port5, motorright, tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port6, motorrightbottom, tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port7, motorleftbottom2, tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor, port8, motorrightbottom2, tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor, port9, motorpuncher2, tmotorVex393_MC29, openLoop)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//

/---------------------------------------------------------------------------/
/* /
/
Description: Competition template for VEX EDR /
/
/
/
---------------------------------------------------------------------------*/

// 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”

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the cortex has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------*/

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.
bStopTasksBetweenModes = true;

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

}

/---------------------------------------------------------------------------/
/* /
/
Autonomous Task /
/
/
/
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. /
/
---------------------------------------------------------------------------/
void moveforward (int speed)
{
slaveMotor(motorleft, motorleftbottom2);
slaveMotor(motorleftbottom, motorleftbottom2);
slaveMotor(motorright, motorrightbottom2);
slaveMotor(motorrightbottom, motorrightbottom2);
motor[motorleftbottom2] = speed;
motor[motorrightbottom2] = speed;
}
void moveleft (int speed)
{
slaveMotor(motorleft, motorleftbottom2);
slaveMotor(motorleftbottom, motorleftbottom2);
slaveMotor(motorright, motorrightbottom2);
slaveMotor(motorrightbottom, motorrightbottom2);
motor[motorleftbottom2] = -speed;
motor[motorrightbottom2] = speed;
}
int inchToTicks (float inch)
{
int ticks;
ticks = inch
42;
return ticks;
}
int degreestoticks (float degree)
{
float radius = 2;
float circumference = 2PIradius;
int ticksperturn = inchToTicks(circumference);
int ticks = ticksperturndegree/360;
return ticks;
}
void PIDturn (float degree, float maxpower = 1)
{
bool timerBool = true;
clearTimer(T1);
float kp = .2;
float ki = 0;
float kd = 0;
int error;
int proportion;
int finalpower;
int lasterror;
int derivative;
int integralRaw;
float integralactivezone;
float integral;
SensorValue[motorright] = 0;
SensorValue[motorleft] = 0;
while(timerBool != false)
{
error = degreestoticks(degree) - (SensorValue[motorright]/2);
proportion = error * kp;
lasterror = error;
derivative = kd
(error - lasterror);
if (error == 0)
{
derivative = 0;
}
integralactivezone = inchToTicks(3);
if(abs(error) <= integralactivezone && error != 0)
{
integralRaw = integralRaw + error;
}
else
{
integralRaw = 0;
}
integral = integralRaw * ki;
finalpower = proportion + integral + derivative;
if(finalpower > maxpower127)
{
finalpower = maxpower
127;
}
else if(finalpower < -maxpower127)
{
finalpower = -maxpower
127;
}
moveleft(finalpower);
wait1Msec(40);
if(error <= inchToTicks(0.5))
{
timerBool = false;
}
}
moveleft(0);
wait1Msec(100);
clearTimer(T1);
}
void PIDforward (int target, float maxpower = 1)
{
float kp = .2;
float ki = 0;
float kd = 0;
int error;
int proportion;
int finalpower;
int lasterror;
int derivative;
int integralRaw;
float integralactivezone;
float integral;
bool timerBool = true;
clearTimer(T1);
SensorValue[motorright] = 0;
SensorValue[motorleft] = 0;
while(timerBool != false)
{
error = inchToTicks(target) - (SensorValue[motorright]);
proportion = error * kp;
lasterror = error;
derivative = kd*(error - lasterror);
if (error == 0)
{
derivative = 0;
}
integralactivezone = inchToTicks(3);
if(error <= integralactivezone && error != 0)
{
integralRaw = integralRaw + error;
}
else
{
integralRaw = 0;
}
integral = integralRaw * ki;
finalpower = proportion + integral + derivative;
if(finalpower > maxpower127)
{
finalpower = maxpower
127;
}
else if(finalpower < -maxpower127)
{
finalpower = -maxpower
127;
}
moveforward(finalpower);
wait1Msec(40);
if(error <= inchToTicks(.5))
{
timerBool = false;
}
}
moveforward(0);
wait1Msec(100);
clearTimer(T1);
}
task autonomous()
{
slaveMotor(motorleft, motorleftbottom2);
slaveMotor(motorleftbottom, motorleftbottom2);
slaveMotor(motorright, motorrightbottom2);
slaveMotor(motorrightbottom, motorrightbottom2);
PIDforward(56,1);
motor[motorintake] = 127;
wait1Msec(2000);
motor[motorintake] = 0;
wait1Msec(1);
PIDforward(-56,1);
PIDturn(-90, 1);
motor[motorpuncher2] = 127;
wait1Msec(1000);
motor[motorpuncher2] = 0;
wait1Msec(1);
PIDforward(28,1);
motor[motorintake] = 127;
wait1Msec(2000);
motor[motorintake] = 0;
wait1Msec(1);
motor[motorpuncher2] = 127;
wait1Msec(1000);
motor[motorpuncher2] = 0;
wait1Msec(1);
PIDforward(28,1);
PIDforward(-84,1);
PIDturn(90, 1);
PIDforward(56, 1);
motor[motorpuncher2] = 0;
motor[motorintake] = 0;
wait1Msec(1);
// …
// Insert user code here.

// ..........................................................................

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

}

/---------------------------------------------------------------------------/
/* /
/
User Control Task /
/
/
/
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. /
/
---------------------------------------------------------------------------*/

task usercontrol()
{
// User control code here, inside the loop

while (true)
{
	slaveMotor(motorleft, motorleftbottom);
	slaveMotor(motorleftbottom2, motorleftbottom);
	slaveMotor(motorright, motorrightbottom);
	slaveMotor(motorrightbottom2, motorrightbottom);
	motor[motorleftbottom] = vexRT[Ch3];
	motor[motorrightbottom] = vexRT[Ch2];
	if (vexRT[Btn6D] == 1)
	{
		motor[motorintake] = 127;
	}
	else if (vexRT[Btn6U] == 1)
	{
		motor[motorintake] = -127;
	}
	else
	{
		motor[motorintake] = 0;
	}
	if (vexRT[Btn5D] == 1)
	{
		motor[motorpuncher2] = 127;
	}
	else if (vexRT[Btn5U] == 1)
	{
		motor[motorpuncher2] = -127;
	}
	else
	{
		motor[motorpuncher2] = 0;
	}
}
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.

// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................

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

}

0 Likes