Committing some more work

14 - Gyroscopes/Challenge/README.md

@@ -0,0 +1,5 @@
+Combine the use of encoders and gyroscopes to do the following
+- Drive forward 60 inches
+- Rotate to 45 degrees (assume a left turn is negative rotation, right turn is positive rotation)
+- Drive forward 20 inches
+- Rotate to -90 degrees

14 - Gyroscopes/Challenge/robot.py

@@ -0,0 +1,79 @@
+import wpilib
+import wpilib.drive
+import math
+import ctre
+
+class MyRobot(wpilib.TimedRobot):
+    def robotInit(self):
+        self.leftFront = ctre.WPI_VictorSPX(0)
+        self.leftRear = ctre.WPI_VictorSPX(1)
+        self.rightFront = ctre.WPI_VictorSPX(2)
+        self.rightRear = ctre.WPI_VictorSPX(3)
+
+        self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
+        self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
+        self.right.setInverted(True)
+
+        self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)
+
+        self.primary = wpilib.XboxController(0)
+
+        self.PULSES_PER_REVOLUTION = 512
+        self.WHEEL_DIAMETER = 6
+        self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION
+
+        self.leftDrivetrainEncoder = wpilib.Encoder(0, 1)
+        self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)
+        self.rightDrivetrainEncoder = wpilib.Encoder(2, 3)
+        self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)
+
+        self.gyro = wpilib.ADXRS450_Gyro()
+        self.gyro.calibrate()
+
+    def autonomousInit(self):
+        self.leftDrivetrainEncoder.reset()
+        self.rightDrivetrainEncoder.reset()
+        self.gyro.reset()
+
+        self.autoState = "DriveForward60"
+
+    def autonomousPeriodic(self):
+        if self.autoState == "DriveForward60":
+            if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 60:
+                self.drivetrain.tankDrive(.5, .5)
+            else: 
+                self.drivetrain.tankDrive(0, 0)
+                self.gyro.reset()
+                self.autoState = "Rotate45"
+        elif self.autoState == "Rotate45":
+            if self.gyro.getAngle() < 45:
+                self.drivetrain.tankDrive(.5, -.5)
+            else:
+                self.drivetrain.tankDrive(0, 0)
+                self.leftDrivetrainEncoder.reset()
+                self.rightDrivetrainEncoder.reset()
+                self.autoState = "DriveForward20"
+        elif self.autoState == "DriveForward20":
+            if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 20:
+                self.drivetrain.tankDrive(.5, .5)
+            else: 
+                self.drivetrain.tankDrive(0, 0)
+                self.gyro.reset()
+                self.autoState = "RotateNegative90"
+        elif self.autoState == "RotateNegative90":
+            if self.gyro.getAngle() > -90:
+                self.drivetrain.tankDrive(-.5, .5)
+            else:
+                self.drivetrain.tankDrive(0, 0)
+                self.autoState = "End"
+        else:
+            self.drivetrain.tankDrive(0, 0)
+
+    def teleopInit(self):
+        pass
+
+    def teleopPeriodic(self):
+        self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

14 - Gyroscopes/robot.py

@@ -0,0 +1,40 @@
+import wpilib
+import wpilib.drive
+import math
+import ctre
+
+class MyRobot(wpilib.TimedRobot):
+    def robotInit(self):
+        self.leftFront = ctre.WPI_VictorSPX(0)
+        self.leftRear = ctre.WPI_VictorSPX(1)
+        self.rightFront = ctre.WPI_VictorSPX(2)
+        self.rightRear = ctre.WPI_VictorSPX(3)
+
+        self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
+        self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
+        self.right.setInverted(True)
+
+        self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)
+
+        self.primary = wpilib.XboxController(0)
+
+        self.gyro = wpilib.ADXRS450_Gyro()
+        self.gyro.calibrate()
+
+    def autonomousInit(self):
+        self.gyro.reset()
+
+    def autonomousPeriodic(self):
+        if self.gyro.getAngle() < 90:
+            self.drivetrain.tankDrive(.5, -.5)
+        else:
+            self.drivetrain.tankDrive(0, 0)
+
+    def teleopInit(self):
+        pass
+
+    def teleopPeriodic(self):
+        self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)