├── README.md ├── odometry.py ├── simple_example.py ├── supersimple_example.py └── teleop_keyboard.py /README.md: -------------------------------------------------------------------------------- 1 | # nz1_python 2 | Examples for controlling the NearZero brushless motor controller from a single-board computer 3 | -------------------------------------------------------------------------------- /odometry.py: -------------------------------------------------------------------------------- 1 | import ctypes 2 | import pylibi2c 3 | import time 4 | 5 | #------CONNECT TO NZ AT GIVEN ADDRESS (e.g. 0x40) AND GIVE IT A NAME (e.g. "wheels")---- 6 | wheels = pylibi2c.I2CDevice('/dev/i2c-1', 0x40) # Open i2c device at address 0x3 7 | wheels.delay = 10 # Set delay 8 | wheels.page_bytes = 16 # Set page_bytes 9 | wheels.flags = pylibi2c.I2C_M_IGNORE_NAK # Set flags 10 | 11 | #------CONNECT TO ANOTHER NZ WITH A DIFFERENT ADDRESS AND NAME--------------------------- 12 | #head = pylibi2c.I2CDevice('/dev/i2c-1', 0x41) # Open i2c device at address 0x4 13 | #head.delay = 10 # Set delay 14 | #head.page_bytes = 16 # Set page_bytes 15 | #head.flags = pylibi2c.I2C_M_IGNORE_NAK # Set flags 16 | 17 | 18 | #------READ FROM THE NEARZERO------------------------------------------------------------ 19 | while True: 20 | data1 = wheels.read(0x0,12) #The last part is the buffer size; enough for 6 digits from each encoder 21 | print(data1) 22 | 23 | #data2 = head.read(0x0,12) 24 | #print(data2) 25 | 26 | #The first 6 digits give the encoder value from channel 1 27 | #The second 6 digits give the encoder value from channel 2 28 | #Digits 1 and 7 indicate sign, where 0 = positive and - = negative. 29 | -------------------------------------------------------------------------------- /simple_example.py: -------------------------------------------------------------------------------- 1 | import ctypes 2 | import pylibi2c 3 | import time 4 | 5 | #-------------------MAKE A CLASS FOR THE NEARZERO---------------------------- 6 | class nz: 7 | def __init__(self, name, address, channel): 8 | self.name = name 9 | self.address = address 10 | self.channel = channel 11 | def i2cSetup(self): 12 | self.address = pylibi2c.I2CDevice('/dev/i2c-1', self.address) 13 | self.address.delay = 10 14 | self.address.page_bytes = 16 15 | self.address.flags = pylibi2c.I2C_M_IGNORE_NAK 16 | def write(self,mode,VelPos,I): 17 | if abs(VelPos)< 10: #make leading zeros depending on size of input 18 | vpleader = "0000" 19 | elif abs(VelPos)< 100: 20 | vpleader = "000" 21 | elif abs(VelPos)< 1000: 22 | vpleader = "00" 23 | elif abs(VelPos)< 10000: 24 | vpleader = "0" 25 | if VelPos >= 0: #choose sign based on value of VelPos 26 | sign = "+" 27 | else: 28 | sign = "-" 29 | VelPos = str(abs(VelPos)) 30 | if I < 10: #make leading zeros depending on size of input 31 | Ileader = "0000" 32 | elif I < 100: 33 | Ileader = "000" 34 | elif I < 1000: 35 | Ileader = "00" 36 | elif I < 10000: 37 | Ileader = "0" 38 | I = str(I) 39 | i2c_out = self.channel+mode+sign+vpleader+VelPos+'c'+Ileader+I #concatenate the channel, sign, and command value 40 | self.address.write(0x0, i2c_out) #Write to ic2 41 | 42 | 43 | #------------INSTANTIATE THE JOINTS--------------------------- 44 | LeftWheel = nz("LeftWheel",0x40,"1") 45 | RightWheel = nz("RightWheel",0x40,"2") 46 | #HeadYaw = nz("HeadYaw",0x41,"1") 47 | #HeadPitch = nz("HeadPitch",0x41,"2") 48 | 49 | #------------START THE I2C SERVICE FOR EACH I2C ADDRESS------- 50 | LeftWheel.i2cSetup() 51 | RightWheel.i2cSetup() 52 | #HeadYaw.i2cSetup() 53 | #HeadPitch.i2cSetup() 54 | 55 | 56 | #-----------MAKE THE NEARZERO DO THINGS----------------------------- 57 | vel1 = 300 #define velocity [unitless] 58 | vel2 = 300 59 | I1 = 200 #define current [mA] 60 | I2 = 200 61 | LeftWheel.write('v',vel1,I1) #write the commands 62 | RightWheel.write('v',vel2,I2) 63 | 64 | #HeadYaw.write('p',-100,100); 65 | #HeadPitch.write('p',200,100); 66 | 67 | 68 | -------------------------------------------------------------------------------- /supersimple_example.py: -------------------------------------------------------------------------------- 1 | import ctypes 2 | import pylibi2c 3 | import time 4 | 5 | #------CONNECT TO NZ AT GIVEN ADDRESS (e.g. 0x40) AND GIVE IT A NAME (e.g. "wheels")------ 6 | wheels = pylibi2c.I2CDevice('/dev/i2c-1', 0x40) 7 | wheels.delay = 10 8 | wheels.page_bytes = 16 9 | wheels.flags = pylibi2c.I2C_M_IGNORE_NAK 10 | 11 | #------CONNECT TO ANOTHER NZ WITH A DIFFERENT ADDRESS AND NAME---------------------------- 12 | #head = pylibi2c.I2CDevice('/dev/i2c-1', 0x41) 13 | #head.delay = 10 14 | #head.page_bytes = 16 15 | #head.flags = pylibi2c.I2C_M_IGNORE_NAK 16 | 17 | 18 | #------WRITE COMMANDS TO NEARZERO--------------------------------------------------------- 19 | #(0x0, 'CHAN(1/2) MODE(v/p/s) SIGN(+/-) CMD CMD CMD CMD CMD "C" CMD CMD CMD CMD CMD') 20 | wheels.write(0x0, '1v+00100c00200') #Drive channel 1 at velocity=100 with 200mA of current 21 | wheels.write(0x0, '2v+00250c00320') #Drive channel 2 at velocity=250 with 320mA of current 22 | 23 | #head.write(0x0, '1p+00000c00000') #Drive channel 1 at position=0 with 0mA of current 24 | #head.write(0x0, '2p+01000c00600') #Drive channel 2 at position=1000 and 600mA of current 25 | 26 | 27 | -------------------------------------------------------------------------------- /teleop_keyboard.py: -------------------------------------------------------------------------------- 1 | import ctypes, pylibi2c, time, sys, termios, tty, os 2 | 3 | #---------------------MAKE A CLASS FOR THE NEARZERO----------------------------------- 4 | class nz: 5 | def __init__(self, name, address, channel): 6 | self.name = name 7 | self.address = address 8 | self.channel = channel 9 | def i2cSetup(self): 10 | self.address = pylibi2c.I2CDevice('/dev/i2c-1', self.address) 11 | self.address.delay = 10 12 | self.address.page_bytes = 16 13 | self.address.flags = pylibi2c.I2C_M_IGNORE_NAK 14 | def write(self,mode,VelPos,I): 15 | if abs(VelPos)< 10: #make leading zeros depending on size of input 16 | vpleader = "0000" 17 | elif abs(VelPos)< 100: 18 | vpleader = "000" 19 | elif abs(VelPos)< 1000: 20 | vpleader = "00" 21 | elif abs(VelPos)< 10000: 22 | vpleader = "0" 23 | if VelPos >= 0: #choose sign based on value of VelPos 24 | sign = "+" 25 | else: 26 | sign = "-" 27 | VelPos = str(abs(VelPos)) 28 | if I < 10: #make leading zeros depending on value of I 29 | Ileader = "0000" 30 | elif I < 100: 31 | Ileader = "000" 32 | elif I < 1000: 33 | Ileader = "00" 34 | elif I < 10000: 35 | Ileader = "0" 36 | I = str(I) 37 | i2c_out = self.channel+mode+sign+vpleader+VelPos+'c'+Ileader+I #concatenate the channel, sign, and command value 38 | self.address.write(0x0, i2c_out) #Write to ic2 39 | 40 | #-------------INSTANTIATE THE JOINTS------------------------------- 41 | LeftWheel = nz("LeftWheel",0x40,"1") 42 | RightWheel = nz("RightWheel",0x40,"2") 43 | #HeadYaw = nz("HeadYaw",0x41,"1") 44 | #HeadPitch = nz("HeadPitch",0x41,"2") 45 | 46 | #-----------START THE I2C SERVICE FOR EACH I2C ADDRESS-------------- 47 | LeftWheel.i2cSetup() 48 | RightWheel.i2cSetup() 49 | #HeadYaw.i2cSetup() 50 | #HeadPitch.i2cSetup() 51 | 52 | 53 | def move(): #This function takes in v_x and v_th and calculates the differential drive velocity for each wheel and writes it to each wheel 54 | vel1 = -1*(v_x + v_th) 55 | vel2 = v_x - v_th 56 | I1 = 1*int(abs(vel1)) #simple current scaling 57 | I2 = 1*int(abs(vel2)) 58 | print("I1="), 59 | print(I1), 60 | print("A") 61 | print("I2="), 62 | print(I2), 63 | print("A") 64 | print(" ") 65 | if vel1 == 0: 66 | I1 = 0 67 | if vel2 == 0: 68 | I2 = 0 69 | LeftWheel.write('v',vel1,I1) 70 | RightWheel.write('v',vel2,I2) 71 | 72 | def getch(): 73 | fd = sys.stdin.fileno() 74 | old_settings = termios.tcgetattr(fd) 75 | try: 76 | tty.setraw(sys.stdin.fileno()) 77 | ch = sys.stdin.read(1) 78 | 79 | finally: 80 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 81 | return ch 82 | 83 | print('w = increment forward velocity') 84 | print('s = increment backward velocity') 85 | print('a = increment left steering') 86 | print('d = increment right steering') 87 | print('x = freeze') 88 | print('') 89 | print('i = point head up') 90 | print('m = point head down') 91 | print('j = point head left') 92 | print('k = point head right') 93 | print('o = head motors off') 94 | print('') 95 | print('q = quit') 96 | 97 | 98 | v_x = 0 99 | v_th = 0 100 | y = 0 101 | p = 0 102 | r = 0 103 | 104 | while True: 105 | char = getch() 106 | if (char == "q"): 107 | exit(0) 108 | if (char == "w"): 109 | v_x = v_x + 20 110 | move() 111 | elif (char == "a"): 112 | v_th = v_th - 10 113 | move() 114 | elif (char == "s"): 115 | v_x = v_x - 20 116 | move() 117 | elif (char == "d"): 118 | v_th = v_th + 10 119 | move() 120 | elif (char == "x"): 121 | v_x = 0 122 | v_th = 0 123 | move() 124 | elif (char == "i"): 125 | p = p + 150 126 | HeadPitch.write('p',p,50) 127 | elif (char == "m"): 128 | p = p - 150 129 | HeadPitch.write('p',p,50) 130 | elif (char == "j"): 131 | y = y - 500 132 | HeadYaw.write('p',y,50) 133 | elif (char == "k"): 134 | y = y + 500 135 | HeadYaw.write('p',y,50) 136 | elif (char == "h"): 137 | r = r - 100 138 | HeadRoll.write('p',r,50) 139 | elif (char == "l"): 140 | r = r + 100 141 | HeadRoll.write('p',r,10) 142 | elif (char == "o"): 143 | HeadPitch.write('p',p,0) 144 | HeadYaw.write('p',y,0) 145 | HeadRoll.write('p',r,0) 146 | 147 | #UNCOMMENT BELOW TO DISPLAY ENCODER DATA EVERY TIME A KEYPRESS IS ENTERED 148 | #odom = LeftWheel.address.read(0x0,12) 149 | #print(odom) 150 | --------------------------------------------------------------------------------