collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
f140848
imu
1import RPIO 2from RPIO import PWM 3from math import copysign 4 5TOP_PIN_1 = 23 6TOP_PIN_2 = 24 7TOP_PIN_PWM = 3 8TOP_ENC_A = 0 9TOP_ENC_B = 0 10 11LEFT_PIN_1 = 7 12LEFT_PIN_2 = 8 13LEFT_PIN_PWM = 2 14LEFT_ENC_A = 0 15LEFT_ENC_B = 0 16 17BOTTOM_PIN_1 = 11 18BOTTOM_PIN_2 = 25 19BOTTOM_PIN_PWM = 18 20BOTTOM_ENC_A = 0 21BOTTOM_ENC_B = 0 22 23RIGHT_PIN_1 = 27 24RIGHT_PIN_2 = 14 25RIGHT_PIN_PWM = 17 26RIGHT_ENC_A = 0 27RIGHT_ENC_B = 0 28 29RPIO .cleanup () 30PWM .cleanup () 31 32class Motor : 33def __init__ (self ,pin1 ,pin2 ,pwm_pin ,encA ,encB ,dma_channel = 0 ): 34self .dma_channel = dma_channel 35self .pwm_pin = pwm_pin 36self .pin1 = pin1 37self .pin2 = pin2 38self .encA = encA 39self .encB = encB 40self .speed = 0 41self .ticks = 0 42# Set up gpio pins, encoder interupts, and dma 43PWM .init_channel (dma_channel ,subcycle_time_us = 8000 ) 44RPIO .setup (pwm_pin ,RPIO .OUT ) 45RPIO .setup (pin1 ,RPIO .OUT ) 46RPIO .setup (pin2 ,RPIO .OUT ) 47RPIO .setup (encA ,RPIO .IN ) 48RPIO .setup (encB ,RPIO .IN ) 49# RPIO.add_interrupt_callback( 50# encA, 51# self.encCallback, 52# edge='rising', 53# threaded_callback=False) # todo: see if this makes a difference 54 55def encCallback (self ,gpioID ,val ): 56temp = RPIO .input (self .encB ) 57if temp : 58self .ticks += 1 59else : 60self .ticks += 1 61# print(self.ticks) 62 63def set (self ,speed ): 64"""manipulates hardware to turn at given speed in range [-1,1] (positive 65should be clockwise)""" 66# todo: verify that positive is clockwise for each motor 67mysign = copysign (1 ,self .speed ) 68newsign = copysign (1 ,speed ) 69self .speed = speed 70if mysign != newsign : 71if newsign == 0 : 72RPIO .output (self .pin1 ,False ) 73RPIO .output (self .pin2 ,False ) 74elif newsign == - 1 : 75RPIO .output (self .pin1 ,False ) 76RPIO .output (self .pin2 ,True ) 77elif newsign == 1 : 78RPIO .output (self .pin2 ,False ) 79RPIO .output (self .pin1 ,True ) 80self .set_speed (abs (self .speed )) 81 82def set_speed (self ,speed ): 83"""adds a pwm pulse for the given speed in range [-1,1]""" 84PWM .add_channel_pulse (self .dma_channel ,self .pwm_pin ,start = 0 ,width = int ( 85speed * PWM .get_channel_subcycle_time_us (self .dma_channel )/ 10 )) 86 87def __str__ (self ): 88return "Speed: " + str (self .speed )+ "\t\tTicks: " + str (self .ticks ) 89 90 91class Base : 92 93def __init__ (self ): 94# self.disable() # is this needed? 95PWM .setup () 96self .top = Motor (TOP_PIN_1 ,TOP_PIN_2 ,TOP_PIN_PWM ,TOP_ENC_A ,TOP_ENC_B ,0 ) 97self .bottom = Motor (BOTTOM_PIN_1 ,BOTTOM_PIN_2 ,BOTTOM_ENC_A ,BOTTOM_ENC_B , 98BOTTOM_PIN_PWM ,1 ) 99self .left = Motor (LEFT_PIN_1 ,LEFT_PIN_2 ,LEFT_PIN_PWM ,LEFT_ENC_A , 100LEFT_ENC_B ,2 ) 101self .right = Motor (RIGHT_PIN_1 ,RIGHT_PIN_2 ,RIGHT_PIN_PWM ,RIGHT_ENC_A , 102RIGHT_ENC_B ,3 ) 103 104def move (self ,x ,y ): 105"""sets motors to move linearly with x and y in range [-1,1]""" 106self .top .set (x ) 107self .bottom .set (- x ) 108self .left .set (y ) 109self .right .set (- y ) 110 111def rotate (self ,speed ): 112"""turns at given speed in range [-1,1]""" 113self .top .set (speed ) 114self .bottom .set (speed ) 115self .left .set (speed ) 116self .right .set (speed ) 117 118def stop (self ): 119self .move (0 ,0 ) 120 121def __str__ (self ): 122return "Top: " + str (self .top )+ "\nBottom: " + str (self .bottom ) \ 123+ "\nLeft: " + str (self .left )+ "\nRight: " + str (self .right ) 124 125def __del__ (self ): 126self .stop () 127PWM .cleanup () 128RPIO .cleanup () 129