Changes between Initial Version and Version 1 of ControlSystems/SoftwareTeam/Training/GettingStarted/ClosedLoopControl


Ignore:
Timestamp:
Oct 30, 2019, 10:39:33 PM (6 years ago)
Author:
David Albert
Comment:

--

Legend:

Unmodified
Added
Removed
Modified
  • ControlSystems/SoftwareTeam/Training/GettingStarted/ClosedLoopControl

    v1 v1  
     1== Closed Loop Control
     2In the last example, we saw why robot programming is challenging: robots work in the real world where wheels slip, motors have different powers, wheels aren't inflated equally, and floors aren't perfectly level.  The robot didn't go straight and didn't stop at exactly 36".  What makes robots interesting and powerful is their ability to sense their environment and respond to it.  In the basic Autonomous example, one wheel was turning faster than the other so the robot turned.  Since the encoders were reporting this, we can modify our program to compensate.  The simplest way to do this is to proportionally increase the motor power of the wheel that's going slower. 
     3
     4MODIFY the last program as follows:
     5
     6* Add the following imports:
     7{{{
     8  import edu.wpi.first.wpilibj.Timer;
     9}}}
     10
     11*  Add variables for closed loop control in the Robot class
     12  {{{
     13    private double  leftPower, rightPower;
     14    private Timer   autoTimer;
     15    private boolean autoDone;
     16  }}}
     17
     18*  In robotInit() instantiate the new Timer object
     19  {{{
     20    autoTimer = new Timer();
     21  }}}
     22
     23* In robotPeriodic() add the motor power settings to the smart dashboard
     24  {{{
     25    SmartDashboard.putNumber("Left power", leftPower);
     26    SmartDashboard.putNumber("Right power", rightPower);
     27  }}}
     28
     29* In autonomousInit() initialize the motor powers and start the timer we created
     30  so we can periodically adjust the motor power to keep the robot going straight:
     31  {{{
     32    // start motors turning forward at 20% power
     33    leftPower = 0.20;
     34    leftMotor.set(leftPower);
     35    // to go forward, left motor turns clockwise, right motor counter-clockwise
     36    rightPower = -0.20;
     37    rightMotor.set(rightPower);
     38
     39    autoTimer.start();
     40    autoDone = false;
     41  }}}
     42
     43* In the default section of autonomousPeriodic() periodically adjust the motor powers
     44  and stop if we've gone the desired distance.
     45  {{{
     46       if (!autoDone) {
     47          // adjust motor power every 1/4 second to keep us going straight
     48          if (autoTimer.hasPeriodPassed(0.25)) {
     49            // if we're going straight, leftDistance == rightDistance
     50            // otherwise, compute the error and adjust the motor powers to minimize it
     51            double error = leftEncoder.getDistance() - rightEncoder.getDistance();
     52            double kP = 0.01; // proportion of error used to correct motor power
     53            // adjust motor power to try to keep left/right distances same
     54            leftPower  -= kP*error; // positive error means left is going too fast
     55            rightPower -= kP*error; // right motor spins opposite direction of left
     56            leftMotor.set(leftPower);
     57            rightMotor.set(rightPower);
     58          }
     59          if ((leftEncoder.getDistance() > 36.0) ||
     60              (rightEncoder.getDistance() > 36.0)) {
     61              leftMotor.stopMotor();
     62              rightMotor.stopMotor();
     63              autoDone = true;
     64          }
     65       }
     66   }}}
     67
     68This is the simplest form of closed-loop control: we are sensing position information (left, right distances) and adjusting powers proportionally (kP) to try to minimize the error.  There are more sophisticated ways to do this that consider how quickly the robot is approaching its goal (derivative of error) and how long and how severely it has been off its target (integral of error).  The PID control loop uses all of these for more precise and responsive closed loop control.
     69
     70Notice that there are two critical values in this control loop:
     71  * How often the control loop runs (every 0.25 seconds in the above example)
     72  * How much the control loop adjusts the power based on the sensed inputs (kP=0.01 in the above example)
     73  * Try printing the error and adjusted motor powers using System.out.println(...) to see what adjustments are taking place.
     74Try experimenting with these values (carefully and changing them a little at a time) to see what effect they have.  Be prepared to hit the Disable button or space bar when testing!
     75