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);
+

Reflections

Written on August 20, 2017