#!/usr/bin/python
# -------------------------------------------
# HBSTEP01B Stepper Motor control test code
# -------------------------------------------
#
# Program uses MLAB Python modules library from https://github.com/MLAB-project/pymlab


#uncomment for debbug purposes
import logging
logging.basicConfig(level=logging.DEBUG) 

import sys
import time
import spidev

#### Script Arguments ###############################################

if len(sys.argv) < 2:
    sys.stderr.write("Invalid number of arguments.\n")
    sys.stderr.write("Usage: %s PORT ADDRESS SPEED MOVE_DISTANCE\n" % (sys.argv[0], ))
    sys.exit(1)

elif len(sys.argv) == 2:
    PORT = eval(sys.argv[1])
    SPEED = 5
    DISTANCE = 50

elif len(sys.argv) == 3:
    SPEED = eval(sys.argv[2])
    DISTANCE = 100

elif len(sys.argv) == 4:
    SPEED = eval(sys.argv[2])
    DISTANCE = eval(sys.argv[3])

else: 
    PORT = 0
    SPEED = 10
    DISTANCE = 50

class axis:
    def __init__(self, SPI_handler, Direction, StepsPerUnit):
        ' One axis of robot '
        self.spi = SPI_handler
        self.Dir = Direction
        self.SPU = StepsPerUnit
        self.Reset()

    def Reset(self):
        ' Reset Axis and set default parameters for H-bridge '
        self.spi.xfer([0xC0])      # reset
#        self.spi.xfer( 0x14)      # Stall Treshold setup
#        self.spi.xfer( 0xFF)  
#        self.spi.xfer( 0x13)      # Over Current Treshold setup 
#        self.spi.xfer( 0xFF)  
        self.spi.xfer([0x15])      # Full Step speed 
        self.spi.xfer([0xFF])
        self.spi.xfer([0xFF]) 
        self.spi.xfer([0x05])      # ACC 
        self.spi.xfer([0x00])
        self.spi.xfer([0x10]) 
        self.spi.xfer([0x06])      # DEC 
        self.spi.xfer([0x00])
        self.spi.xfer([0x10]) 
        self.spi.xfer([0x0A])      # KVAL_RUN
        self.spi.xfer([0xFF])
        self.spi.xfer([0x0B])      # KVAL_ACC
        self.spi.xfer([0xFF])
        self.spi.xfer([0x0C])      # KVAL_DEC
        self.spi.xfer([0xFF])
        self.spi.xfer([0x18])      # CONFIG
        self.spi.xfer([0b00111000])
        self.spi.xfer([0b00000000])
      
    def MaxSpeed(self, speed):
        ' Setup of maximum speed '
        self.spi.xfer([0x07])       # Max Speed setup 
        self.spi.xfer([0x00])
        self.spi.xfer([speed])  

    def ReleaseSW(self):
        ' Go away from Limit Switch '
        while self.ReadStatusBit(2) == 1:           # is Limit Switch ON ?
            self.spi.xfer([0x92 | (~self.Dir & 1)])     # release SW 
            while self.IsBusy():
                pass
            self.MoveWait(10)           # move 10 units away
 
    def GoZero(self, speed):
        ' Go to Zero position '
        self.ReleaseSW()

        self.spi.xfer([0x82 | (self.Dir & 1)])       # Go to Zero
        self.spi.xfer([0x00])
        self.spi.xfer([speed])  
        while self.IsBusy():
            pass
        time.sleep(0.3)
        self.ReleaseSW()

    def Move(self, units):
        ' Move some distance units from current position '
        steps = units * self.SPU  # translate units to steps 
        if steps > 0:                                          # look for direction
            self.spi.xfer([0x40 | (~self.Dir & 1)])       
        else:
            self.spi.xfer([0x40 | (self.Dir & 1)]) 
        steps = int(abs(steps))     
        self.spi.xfer([(steps >> 16) & 0xFF])
        self.spi.xfer([(steps >> 8) & 0xFF])
        self.spi.xfer([steps & 0xFF])

    def MoveWait(self, units):
        ' Move some distance units from current position and wait for execution '
        self.Move(units)
        while self.IsBusy():
            pass
            time.sleep(0.8)

    def Float(self):
        ' switch H-bridge to High impedance state '
        self.spi.xfer([0xA0])

    def ReadStatusBit(self, bit):
        ' Report given status bit '
        self.spi.xfer([0x39])   # Get status command
        data = self.spi.readbytes(1)           # 1st byte
        data = data + (self.spi.readbytes(1))           # 1st byte
        print data
        if bit > 7:                                   # extract requested bit
            OutputBit = (data[0] >> (bit - 8)) & 1
        else:
            OutputBit = (data[1] >> bit) & 1        
        return OutputBit


    
    def IsBusy(self):
        """ Return True if tehre are motion """
        if self.ReadStatusBit(1) == 1:
            return False
        else:
            return True

# End Class axis --------------------------------------------------

print "Stepper motor control test started. \r\n"
print "Max motor speed: %d " % SPEED
print "Distance to run: %d " % DISTANCE

try:
    print "SPI configuration.."
    spi = spidev.SpiDev() # create a spi object
    spi.open(0, 0) # open spi port 0, device (CS) 0 
    spi.mode = 0b01
    spi.lsbfirst = False
    spi.bits_per_word = 8
    spi.cshigh = False
    #spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)
    time.sleep(1)

    print "Axis inicialization"
    X = axis(spi, 0, 641)    # set Number of Steps per axis Unit and set Direction of Rotation
    X.MaxSpeed(SPEED)                      # set maximal motor speed 

    print "Axis is running"

    for i in range(5):
        print i
        X.MoveWait(DISTANCE)      # move forward and wait for motor stop
        print "Changing direction of rotation.."
        time.sleep(1.1)
        X.MoveWait(-DISTANCE)     # move backward and wait for motor stop
        print "Changing direction of rotation.."
        time.sleep(1.1)
    X.Float()   # release power


finally:
    print "stop"