@jpearman We’re trying to use PID system for our robot but the error is not correcting the flywheel speed and whenever we press a button the flywheel goes on an on non-stop full speed. Is there anything we can do to make it work better?
#pragma config(Sensor, dgtl1, wheelEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl12, greenLED, sensorLEDtoVCC)
#pragma config(Motor, port1, liftRight, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, frontLeft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port3, frontRight, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port4, backLeft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port5, backRight, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port6, shootLeft, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port7, shootRight, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port8, shootLeftTwo, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port9, shootRightTwo, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port10, liftLeft, tmotorVex393_HBridge, openLoop)
int target=0; //define target and rpm at global level so it can be passed between tasks
float rpm;
int currentEncoder; //Define variables to use
int previousEncoder;
int deltaEncoder;
int currentTime;
int previousTime;
int deltaTime;
task speedControl(); //define tasks to be used
task speedCalculate();
task main()
{
SensorValue[greenLED]=0;
startTask(speedControl);
startTask(speedCalculate);
while (true)
{
motor[frontRight] = vexRT[Ch3] - vexRT[Ch1] - vexRT[Ch4];
motor[backRight] = vexRT[Ch3] - vexRT[Ch1] + vexRT[Ch4];
motor[frontLeft] = vexRT[Ch3] + vexRT[Ch1] + vexRT[Ch4];
motor[backLeft] = vexRT[Ch3] + vexRT[Ch1] - vexRT[Ch4];
if(vexRT[Btn6UXmtr2] == 1)
{
motor[liftLeft] = -127;
motor[liftRight] = -127;
target = 400;
}
else
{
if(vexRT[Btn5UXmtr2] == 1)
{
motor[liftLeft] = -127;
motor[liftRight] = -127;
}
else
{
if(vexRT[Btn5DXmtr2] == 1)
{
motor[liftLeft] = 127;
motor[liftRight] = 127;
}
else
{
motor[liftLeft] = 0;
motor[liftRight] = 0;
}
}
}
if(vexRT[Btn6DXmtr2] == 1)
{
target = 200;
}
if(vexRT[Btn8UXmtr2] == 1)
{
target = 150;
}
if(vexRT[Btn8RXmtr2] == 1)
{
target = 100;
}
if(vexRT[Btn8DXmtr2] == 1)
{
target = 75;
}
if(vexRT[Btn8LXmtr2] == 1)
{
target = 50;
}
if(vexRT[Btn7RXmtr2] == 1)
{
target = 0;
}
if(abs(target-rpm)<15) //Use LED to indicate when velocity is at target
{
SensorValue[greenLED]=1;
}
else
{
SensorValue[greenLED]=0;
}
}
}
task speedCalculate()
{
while(true)
{
previousEncoder = SensorValue[wheelEncoder]; //Measre Encoder and time at beginning
previousTime = nSysTime;
wait1Msec(25); //Wait to measre changes
currentEncoder = SensorValue[wheelEncoder]; //Measure Encoder and time after delay
currentTime = nSysTime;
deltaEncoder = currentEncoder - previousEncoder; //Calculate changes
deltaTime = currentTime - previousTime;
rpm = (deltaEncoder*60)*(1000/deltaTime)/360; //calculate rpm but converrt degress to rotations, ms to s, s to minutes
wait1Msec(10); //Wait to allow
}
}
task speedControl()
{
float kp = 0.9; //Define Variables to use
float kd = 0.07;
float ki = 0.03;
int error = 0;
int previousError = 0;
int integral = 0;
int derivative = 0;
int motorSpeed = 0;
while(true)
{
error = target - rpm; //Determine how far away from target rpmis
integral += error; //Accumulate Error to determine steady-state answer
derivative = error - previousError; //Determine rate error is changing
previousError = error;
motorSpeed = kp*error + ki*integral + kd*derivative; //Use PID loop to determine motorspeed
if(motorSpeed<0) //Keep motors from going backward
{
motorSpeed = 0;
}
if(motorSpeed>127) //Keep Motorspeed from going over 127
{
motorSpeed = 127;
}
motor[shootLeft] = motorSpeed; //Set Motors
motor[shootRight] = motorSpeed;
motor[shootLeftTwo] = motorSpeed; //Set Motors
motor[shootRightTwo] = motorSpeed;
wait1Msec(40); //Delay for motor speed to adjust to RPM
}
}