Fix Gyro code examples (#1725)

Invert right side of drive
Reduce P in turn to angle to a more reasonable value
Fix sign so the turn to angle actually turns

Author: sciencewhiz

source/docs/software/hardware-apis/sensors/gyros-software.rst

@@ -191,6 +191,11 @@ The following example shows how to stabilize heading using a simple P loop close
 
         DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);
 
+        @Override
+        public void robotInit() {
+            rightMotors.setInverted(true);
+        }
+
         @Override
         public void autonomousPeriodic() {
             // Setpoint is implicitly 0, since we don't want the heading to change
@@ -218,6 +223,10 @@ The following example shows how to stabilize heading using a simple P loop close
 
         frc::DifferentialDrive drive{leftMotors, rightMotors};
 
+        void Robot::RobotInit() {
+          rightMotors.SetInverted(true);
+        }
+
         void Robot::AutonomousPeriodic() {
             // Setpoint is implicitly 0, since we don't want the heading to change
             double error = -gyro.GetRate();
@@ -257,6 +266,11 @@ The following example shows how to stabilize heading using a simple P loop close
 
         DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);
 
+        @Override
+        public void robotInit() {
+            rightMotors.setInverted(true);
+        }
+
         @Override
         public void autonomousInit() {
             // Set setpoint to current heading at start of auto
@@ -292,6 +306,10 @@ The following example shows how to stabilize heading using a simple P loop close
 
         frc::DifferentialDrive drive{leftMotors, rightMotors};
 
+        void Robot::RobotInit() {
+          rightMotors.SetInverted(true);
+        }
+
         void Robot::AutonomousInit() {
             // Set setpoint to current heading at start of auto
             heading = gyro.GetAngle();
@@ -320,7 +338,7 @@ Much like with heading stabilization, this is often accomplished with a PID loop
         // Use gyro declaration from above here
 
         // The gain for a simple P loop
-        double kP = 1;
+        double kP = 0.05;
 
         // Initialize motor controllers and drive
         Spark left1 = new Spark(0);
@@ -334,21 +352,26 @@ Much like with heading stabilization, this is often accomplished with a PID loop
 
         DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);
 
+        @Override
+        public void robotInit() {
+            rightMotors.setInverted(true);
+        }
+
         @Override
         public void autonomousPeriodic() {
             // Find the heading error; setpoint is 90
             double error = 90 - gyro.getAngle();
 
             // Turns the robot to face the desired direction
-            drive.tankDrive(kP * error, kP * error);
+            drive.tankDrive(kP * error, -kP * error);
         }
 
     .. code-tab:: c++
 
         // Use gyro declaration from above here
 
         // The gain for a simple P loop
-        double kP = 1;
+        double kP = 0.05;
 
         // Initialize motor controllers and drive
         frc::Spark left1{0};
@@ -361,12 +384,16 @@ Much like with heading stabilization, this is often accomplished with a PID loop
 
         frc::DifferentialDrive drive{leftMotors, rightMotors};
 
+        void Robot::RobotInit() {
+          rightMotors.SetInverted(true);
+        }
+
         void Robot::AutonomousPeriodic() {
             // Find the heading error; setpoint is 90
             double error = 90 - gyro.GetAngle();
 
             // Turns the robot to face the desired direction
-            drive.TankDrive(kP * error, kP * error);
+            drive.TankDrive(kP * error, -kP * error);
         }
 
 As before, more-advanced implementations can use more-complicated control loops.