slew_rate_all

src/pi/Adafruit_PWM_Servo_Driver.py

@@ -45,7 +45,7 @@ def __init__(self, address=0x40, debug=False):
     self.address = address
     self.debug = debug
     if (self.debug):
-      print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
+      print("Reseting PCA9685 MODE1 (without SLEEP) and MODE2")
     self.setAllPWM(0, 0)
     self.i2c.write8(self.__MODE2, self.__OUTDRV)
     self.i2c.write8(self.__MODE1, self.__ALLCALL)
@@ -63,11 +63,11 @@ def setPWMFreq(self, freq):
     prescaleval /= float(freq)
     prescaleval -= 1.0
     if (self.debug):
-      print "Setting PWM frequency to %d Hz" % freq
-      print "Estimated pre-scale: %d" % prescaleval
+      print("Setting PWM frequency to %d Hz" % freq)
+      print("Estimated pre-scale: %d" % prescaleval)
     prescale = math.floor(prescaleval + 0.5)
     if (self.debug):
-      print "Final pre-scale: %d" % prescale
+      print("Final pre-scale: %d" % prescale)
 
     oldmode = self.i2c.readU8(self.__MODE1);
     newmode = (oldmode & 0x7F) | 0x10             # sleep

src/pi/config.ini

@@ -0,0 +1,4 @@
+[Main]
+TeamName = Thursday
+Manip4 = INTAKEJ
+Manip5 = ARM

src/pi/server.py

@@ -38,7 +38,11 @@
 RIGHT_MOT = 1
 LEFT_MANIP = 4
 RIGHT_MANIP = 5
+RTY = 2
 
+MANIP_TYPES = list(map(str, [config.get('Main', 'Manip4'), config.get('Main', 'Manip5')]))
+#INTAKEJ(Intake joystick), ARM(arm), "INTAKET"(Intake trigger), "DUAL_INTAKE"(Old ARM/INTAKE) - first goes to 4, second to 5
+print(MANIP_TYPES)
 pwm = PWM(0x40)
 
 
@@ -50,21 +54,21 @@ def setServoPulse(channel, pulse):
     # print "%d us per bit" % pulseLength
     pulse /= pulseLength
     # print "%d tick" % pulse
-    pwm.setPWM(channel, 0, pulse)
+    pwm.setPWM(int(channel), 0, int(pulse))
 
 
 # loging wrapper function
 def logWrite(strng):
     log.write("[" + str(time.time()) + "]" + strng + "\n")
-    print strng
+    print(strng)
 
 
 # Error log file, overwtite file from last reset
 logFile = "./elog_" + str(time.time()) + ".txt"
 try:
     log = open(logFile, "w")
 except Exception as f:
-    print "Uhhh... check that your hard drive isn't on fire!"
+    print("Uhhh... check that your hard drive isn't on fire!")
     sys.exit(1)
 else:
     logWrite("log File" + logFile)
@@ -104,7 +108,7 @@ def pwmControlThread():
 
     # thread main loop
     while True:
-	time.sleep(.005)
+        time.sleep(.005)
         # check for data that needs to be bridged to arduino
         dataFlag = True
         if not serialRealTimeQueue.empty():
@@ -114,12 +118,13 @@ def pwmControlThread():
 
         # if data was recieved parse and update pwm hat
         if dataFlag:
-            data = data[5:-1]
-            print data
+            print(data) #Check if we are cutting out inputs
+            data = data[5:-1] #0 left y, 1 right x, 2 left trigger, 3 right trigger, 4 RB, 5 LB?
+            print(data)
             data_nums = [int(x) for x in data.split(':') if x.strip()]
-            print " ", data_nums[0], " ", data_nums[1]
+            print(" ", data_nums[0], " ", data_nums[1])
             leftMtr,rightMtr = t.transform(data_nums[0],data_nums[1])
-            print " ", leftMtr , " ", rightMtr
+            print(" ", leftMtr , " ", rightMtr)
             setServoPulse(LEFT_MOT, leftMtr)
             setServoPulse(RIGHT_MOT, rightMtr)
 
@@ -130,17 +135,55 @@ def pwmControlThread():
 
             leftIntake = Transform.MOTOR_IDLE
             rightIntake = Transform.MOTOR_IDLE
-            if data_nums[2] > 127 + 10: # if left trigger pressed (i think) spin motors in opposite direction
-                leftIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
-                rightIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
-            elif data_nums[3] > 127 + 10: # if right trigger pressed
-                leftIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
-                rightIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
-            else :
-                leftIntake = Transform.MOTOR_IDLE
-                rightIntake = Transform.MOTOR_IDLE
+
+            if "INTAKEJ" in MANIP_TYPES:
+                #IntakeJStick: -> rightY -> Motor 
+                rstickY = RTY
+                if (data_nums[rstickY] >= 127 + 17):
+                    leftIntake = Transform.MOTOR_IDLE + INTAKE_ABS_SPD
+                elif (data_nums[rstickY] <= 127 + 17):
+                    leftIntake =  Transform.MOTOR_IDLE - INTAKE_ABS_SPD
+                else:
+                    leftIntake = Transform.MOTOR_IDLE
+
+            if "INTAKE" in MANIP_TYPES:
+                #Intake: -> RB/LB -> Motor on / off
+                RB = 4
+                LB = 5
+                ON = 200
+                INTAKE_ABS_SPD = 400
+                if (data_nums[RB] == ON):
+                    leftIntake = Transform.MOTOR_IDLE + INTAKE_ABS_SPD
+                elif (data_nums[LB] == ON):
+                    leftIntake =  Transform.MOTOR_IDLE - INTAKE_ABS_SPD
+                else:
+                    leftIntake = Transform.MOTOR_IDLE
+
+            if "ARM" in MANIP_TYPES:
+                #Arm: Triggers -> One motor slow
+                LT = 2
+                RT = 3
+                BOUND = 400
+                if data_nums[LT] > 127 + 10: # if left trigger pressed (i think) spin motors in opposite direction
+                    rightIntake = Transform.map_range(data_nums[LT],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN + BOUND)
+                elif data_nums[RT] > 127 + 10: # if right trigger pressed
+                    rightIntake = Transform.map_range(data_nums[RT],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX - BOUND)
+                else :
+                    rightIntake = Transform.MOTOR_IDLE
+
+            if "DUAL_INTAKE" in MANIP_TYPES:
+                #Triggers -> One motor
+                if data_nums[2] > 127 + 10: # if left trigger pressed (i think) spin motors in opposite direction
+                    leftIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
+                    rightIntake = Transform.map_range(data_nums[2],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
+                elif data_nums[3] > 127 + 10: # if right trigger pressed
+                    leftIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MIN)
+                    rightIntake = Transform.map_range(data_nums[3],127,255,Transform.MOTOR_IDLE,Transform.MOTOR_MAX)
+                else :
+                    leftIntake = Transform.MOTOR_IDLE
+                    rightIntake = Transform.MOTOR_IDLE
 
-            print " " , leftIntake, " ", rightIntake
+            print(" " , leftIntake, " ", rightIntake)
 
             setServoPulse(LEFT_MANIP,leftIntake)
             setServoPulse(RIGHT_MANIP,rightIntake)
@@ -154,7 +197,7 @@ def pwmControlThread():
             setServoPulse(RIGHT_MANIP, Transform.MOTOR_IDLE)
 
             watchdog = time.time()
-            print "you need to feed the dogs"
+            print("you need to feed the dogs")
 
 def networkComThread():
 

src/pi/transform.py

@@ -8,6 +8,9 @@ class Transform:
     MOTOR_MAX = 2000
     L_STICK_DEADZONE = 17
     R_STICK_DEADZONE = 17
+    SLEW_LIMIT_ON = True
+    SLEW_LIMIT = 42.0
+    last_lefty = 0
 
     def __init__(self,invert_left_axis,invert_right_axis):
         self.invert_left = invert_left_axis
@@ -17,19 +20,43 @@ def __init__(self,invert_left_axis,invert_right_axis):
     def map_range(x, in_min, in_max, out_min, out_max):
         return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
-
-
+    @staticmethod
+    def slew_limit(input, last, limit):
+        output = input
+        if abs(last - input) > limit:
+            print("slewey")
+            if last <= 127:
+                if (input > last):
+                    if (input > 127) and (input - 127 > limit):
+                        output = limit
+                    else:
+                        output = input
+                else:
+                    output = last - limit
+            else:
+                if (input > last):
+                    output = last - limit
+                else:
+                    if (input < 127) and (127 - input > limit):
+                        output = -limit
+                    else:
+                        output = input
+        return output
 
     def transform(self, left_y, right_x):
+        if self.SLEW_LIMIT_ON:
+            left_y = Transform.slew_limit(left_y, self.last_lefty, Transform.SLEW_LIMIT)
+            self.last_leftx = left_y
+
         if 127-17 <= left_y <= 127+17:
-		left_y = Transform.MOTOR_IDLE
-	else:
-		left_y = map_range(left_y, 0, 255, Transform.MOTOR_MIN, Transform.MOTOR_MAX)
+            left_y = Transform.MOTOR_IDLE
+        else:
+            left_y = map_range(left_y, 0, 255, Transform.MOTOR_MIN, Transform.MOTOR_MAX)
 
-	if 127-17 <= right_x <= 127+17:
-		right_x = Transform.MOTOR_IDLE
-	else:
-		right_x = map_range(right_x, 0, 255, Transform.MOTOR_MIN, Transform.MOTOR_MAX)
+        if 127-17 <= right_x <= 127+17:
+            right_x = Transform.MOTOR_IDLE
+        else:
+            right_x = map_range(right_x, 0, 255, Transform.MOTOR_MIN, Transform.MOTOR_MAX)
 
         lftMtr = Transform.MOTOR_IDLE
         rghtMtr = Transform.MOTOR_IDLE
@@ -80,5 +107,5 @@ def transform(self, left_y, right_x):
         return lftMtr,rghtMtr
 
 t = Transform(False,False)
-print t.transform(255,255)
+print(t.transform(255,255))