Worm Class for worm robot

worm.py

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)