So I have been trying to create a PID for the tray on our robot but for some reason, the code does not work… I was wondering if anyone would be able to tell me why when I press the tray buttons, the tray only moves backwards and continues to move backwards even when I let go of the buttons. Any help would be appreciated!
Code:
/*---------------------------------------------------------------------------*/ #include "robot-config.h" vex::competition Competition; /*-------------------------------Variables-----------------------------------*/ double ColumnTarget = 200; double ColumnDesired; double ColumnKP = 0.0; double ColumnPreviousError = 0.0; double ColumnDerivative = 0.0; double ColumnKD = 0.0; double ColumnIntegral = 0.0; double ColumnKI = 0.0; double ColumnError; `/*---------------------------------------------------------------------------*/` /* Pre-Autonomous Functions */ /*---------------------------------------------------------------------------*/ void pre_auton( void ) { } /*---------------------------------------------------------------------------*/ /* Autonomous Task */ /*---------------------------------------------------------------------------*/ void autonomous( void ) { } /*----------------------------------------------------------------------------*/ /* User Control Task */ /*----------------------------------------------------------------------------*/ void usercontrol( void ) { while (1){ if(Controller1.ButtonB.pressing()) { ColumnDesired = 20; ColumnKP = 0.02; ColumnKD = 0; ColumnKI = 0.0; } else if(Controller1.ButtonA.pressing()) { ColumnDesired = 0; ColumnKP = 0.02; ColumnKD = 0; ColumnKI = 0; } ColumnError = pow((TrayTilter.rotation(rotationUnits::deg) - ColumnDesired),3); ColumnDerivative = ColumnError - ColumnPreviousError; if (((TrayTilter.rotation(rotationUnits::deg) - ColumnDesired) < 5.0) && ((TrayTilter.rotation(rotationUnits::deg) - ColumnDesired) > -5.0)){ ColumnIntegral = 0; } else { ColumnIntegral += ColumnError; TrayTilter.spin(fwd, (ColumnError * ColumnKP + ColumnDerivative * ColumnKD + ColumnIntegral * ColumnKI), voltageUnits::volt); } ColumnPreviousError = ColumnError; vex::task::sleep(20); } } int main() { pre_auton(); Competition.autonomous( autonomous ); Competition.drivercontrol( usercontrol ); while(1) { vex::task::sleep(20); } }