Balancing and PID

Tags: control and software
Personhours: 5
Balancing and PID By Tycho

Task: Test and improve the PID system and balance code

We're currently testing code to give Argos a balancing system so that we can demo it. This is also a test for the PID in the new REV robotics expansion hubs, which we plan on switching to for this season if reliable. Example code is below. 

public void BalanceArgos(double Kp, double Ki, double Kd, double pwr, double currentAngle, double targetAngle)
 {
     //sanity check - exit balance mode if we are out of recovery range
 
 
 
     if (isBalanceMode()){ //only balance in the right mode
 
         setHeadTilt(nod);
 
         //servo steering should be locked straight ahead
         servoSteerFront.setPosition(.5);
         servoSteerBack.setPosition(0.5);
 
         //double pwr = clampMotor((roll-staticBalance)*-.05);
 
         balancePID.setOutputRange(-.5,.5);
         balancePID.setPID(Kp, Ki, Kd);
         balancePID.setSetpoint(staticBalance);
         balancePID.enable();
         balancePID.setInput(currentAngle);
         double correction = balancePID.performPID();
 
         logger.UpdateLog(Long.toString(System.nanoTime()) + ","
                 + Double.toString(balancePID.getDeltaTime()) + ","
                 + Double.toString(currentAngle) + ","
                 + Double.toString(balancePID.getError()) + ","
                 + Double.toString(balancePID.getTotalError()) + ","
                 + Double.toString(balancePID.getDeltaError()) + ","
                 + Double.toString(balancePID.getPwrP()) + ","
                 + Double.toString(balancePID.getPwrI()) + ","
                 + Double.toString(balancePID.getPwrD()) + ","
                 + Double.toString(correction));
 
         timeStamp=System.nanoTime();
         motorFront.setPower(correction);

Date | August 20, 2017