!!!!!!!!!!!!!!

aimer.py

@@ -42,7 +42,7 @@ def getYaw(self):
 
         # def getAngle(self):
         ##    self.ag = abs(self.gyro.getAngle()) % 360
-        return self.ag
+        #return self.ag
 
     def calculate(self, m):
         return (self.turnController.calculate(m))
@@ -51,6 +51,8 @@ def calculate(self, m):
     #    self.setRotateToAngleRate = output
 
     def calcDiff(self, theta):
+        if(not theta):
+            return None
         angle = self.getYaw()
         return(abs(angle - theta))
 
@@ -65,12 +67,12 @@ def calculateDriveSpeeds(self, theta):
         # rotationRate = self.turnController.calculate(angle, theta)
         # return(rotationRate, 0)
 
-        diff = calcDiff(theta)
+        diff = self.calcDiff(theta)
         correctionFactor = (diff / 10.0)
         if correctionFactor > 1.0:
             correctionFactor = 1.0
 
-        if (getInRange(diff)):
+        if (self.getInRange(diff)):
             print("diff <= ", self.accuracyDegrees)
             return (0, 0)
         else:

climber.py

@@ -15,21 +15,32 @@ def __init__(self, solenoidGroup, winch, extendSpeed, retractSpeed):
         self.winch = winch
         self.climbstep = 0
         # Actions to Climb
-        self.climbActions = [[self.winchForward, 0.5], [self.pistonForward, 0.5], [self.winchForward, 2.5],
-                             [self.pistonReverse, 0.5], [self.winchReverse, 3.0], [self.winchOff, 0.1]]
+        self.climbActions = [[self.winchExtend, 0.5], [self.pistonForward, 0.5], [self.winchExtend, 2.5],
+                             [self.pistonReverse, 0.5], [self.winchRetract, 3.0], [self.winchOff, 0.1]]
         self.extendSpeed = extendSpeed
         self.retractSpeed = retractSpeed
 
     def climberOff(self):
          self.winch.set(0.0)  
          self.solenoids.set(kOff) 
 
-    def setWinch(self, x): # positive corresponds to extend, negative corresponds to retract
-        print("setWinch = ", x)
-        if x >= 0: 
-            self.winch.set(self.extendSpeed*x)
+    def getWinch(self):
+        return(self.winch.get())
+
+    def setWinch(self, leftSpeedMod, rightSpeedMod):
+
+        # positive corresponds to extend, negative corresponds to retract
+        if (leftSpeedMod >= 0):
+            leftSpeed = self.extendSpeed * leftSpeedMod
         else:
-            self.winch.set(self.retractSpeed*x)
+            leftSpeed = self.retractSpeed * leftSpeedMod
+
+        if (rightSpeedMod >= 0):
+            rightSpeed = self.extendSpeed * rightSpeedMod
+        else: 
+            rightSpeed = self.retractSpeed * rightSpeedMod
+
+        self.winch.set(leftSpeed, rightSpeed)
 
     def pistonForward(self):
         self.solenoids.set(kForward)
@@ -75,7 +86,7 @@ def toggle(self):
             s.toggle()
 
 class WinchGroup:
-    def __init__(self, right_winch, left_winch, right_limit, left_limit, cable_wrapped)
+    def __init__(self, right_winch, left_winch, right_limit, left_limit, cable_wrapped):
         self.right_winch = right_winch
         self.left_winch = left_winch
 
@@ -97,21 +108,33 @@ def __init__(self, right_winch, left_winch, right_limit, left_limit, cable_wrapp
         # makes right winch spin in negative (clockwise)
         # which draws the cable down
 
-    def set(speed):
-        speed = speed*self.winch_mod
-
+    def get(self):
+        return (self.right_winch.get(), self.left_winch.get())
+
+    def set(self, left_speed, right_speed):
+
         # meaning right motor not allowed to spin clockwise (negative) more
         # assuming cable wrapped under
 
-        if self.atRightLimit() or self.atLeftLimit():
-            if speed < 0:
-                speed = 0
+        leftSpeed = left_speed * self.winch_mod
+        rightSpeed = right_speed * self.winch_mod
+
+        '''
+        #print("limits: ", self.atRightLimit(), self.atLeftLimit())
+        if self.atRightLimit():
+            if rightSpeed > 0:
+                rightSpeed = 0
+        if self.atLeftLimit():
+            if leftSpeed > 0:
+                leftSpeed = 0
+        '''
+
 
-        self.right_winch.set(speed)
-        self.left_winch.set(-speed)
+        self.right_winch.set(-rightSpeed)
+        self.left_winch.set(-leftSpeed)
 
     def atRightLimit(self):
-        return self.right_limit.get()
+        return not(self.right_limit.get())
 
     def atLeftLimit(self):
-        return self.left_limit.get()
+        return not(self.left_limit.get())

feeder.py

@@ -1,27 +1,29 @@
 import wpilib
 
+
 class Feeder:
-    def __init__(self, motor_controller):
+    def __init__(self, motor_controller, feederSpeed):
         self.motor = motor_controller
+        self.feederSpeed = feederSpeed
         self.motor.setClosedLoopRampRate(1.0)
         self.encoder = self.motor.getEncoder()
-        self.encoder.setPosition(0) # Reset position of motor to zero
+        self.encoder.setPosition(0)  # Reset position of motor to zero
 
     def setFeeder(self, speed):
+        print("Setting feeder speed: ", speed)
         self.motor.set(speed)
 
     def setPosition(self, position):
         pass
-    
+
     def getPosition(self):
-        return(self.encoder.getPosition())
+        return self.encoder.getPosition()
 
     def hasFired(self):
-        if(self.getPosition() > 10):
+        if self.getPosition() > 10:
             return True
         else:
             return False
 
     def reset(self):
         self.setPosition(0)
-

intake.py

@@ -4,15 +4,17 @@
 kForward = DoubleSolenoid.Value.kForward
 kReverse = DoubleSolenoid.Value.kReverse
 
+
 # Simplified version of the Climber class
 class Intake:
-    def __init__(self, doubleSolenoid, motor, motorSpeed = 0.5):
+    #def __init__(self, doubleSolenoid, motor, motorSpeed=0.5):
+    def __init__(self, doubleSolenoid):
         self.solenoid = doubleSolenoid
-        self.motor = motor
-        self.motorSpeed = motorSpeed
+        #self.motor = motor
+        #self.motorSpeed = motorSpeed
 
         self.solenoid.set(kOff)
-    
+
     def toggle(self):
         """
         Toggles the state of the piston between kForward and kReverse.
@@ -28,9 +30,10 @@ def extend(self):
 
     def retract(self):
         self.solenoid.set(kReverse)
-
+    '''
     def motorOn(self):
         self.motor.set(self.motorSpeed)
 
     def motorOff(self):
-        self.motor.set(0.0)
+        self.motor.set(0.0)
+    '''