Created
May 9, 2017 03:22
-
-
Save flyboy74/692692502eb80349215b43354d23e988 to your computer and use it in GitHub Desktop.
Worm Class for worm robot
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import smbus | |
| import time | |
| class Worm : | |
| global bus | |
| global address | |
| bus = smbus.SMBus(1) | |
| address = 0x40 | |
| @classmethod | |
| def __init__(self): | |
| bus.write_byte_data(address, 0x01, 0x04) | |
| bus.write_byte_data(address, 0x00, 0x01) | |
| time.sleep(0.005) | |
| oldreg0 = bus.read_byte_data(address, 0x00) | |
| newreg0 = (oldreg0 & 0x7F) | 0x10 | |
| bus.write_byte_data(address, 0, newreg0) | |
| prescale = int((25000000/(4096*50))-1) | |
| bus.write_byte_data(address, 0xFE, prescale) | |
| bus.write_byte_data(address, 0, oldreg0) | |
| time.sleep(0.005) | |
| bus.write_byte_data(address, 0, oldreg0 | 0x80) | |
| bus.write_byte_data(address, 0xFA, 0) | |
| bus.write_byte_data(address, 0xFB, 0) | |
| bus.write_byte_data(address, 0xFC, 0x33) | |
| bus.write_byte_data(address, 0xFD, 0x01) | |
| def set_muscle (self,muscle_number, position): | |
| step = 202/90 | |
| if position < -90 : position = -90 | |
| if position > 90 : position = 90 | |
| if position == 0 : offtime = 307 | |
| else : offtime = 307 + (step * position) | |
| startreg = muscle_number * 4 + 8 | |
| bus.write_byte_data(address, startreg, offtime & 0xff) | |
| bus.write_byte_data(address, startreg+1, offtime >> 8) | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment