# Motorized Resistive Load - Python Code by Abdallah Obidat

# Note that the Raspberry Pi was configured to boot into this program. This can be achieved in a number of ways.

import smbus as i2cBUS  # import smbus to allow control of the I2C bus
import RPi.GPIO as GPIO  # import RPi.GPIO to allow control of the GPIO pins
import time as tm  # import time to allow the insertion of time delays in the code

bus = i2cBUS.SMBus()  # create a SMBus object
bus.open(1)  # open communication line of the i2c bus object - port 1 is being used here

display_register_address = 0x3C
tm.sleep(5)  # wait for two seconds after initializing the i2c bus

# the last value turns the display on or off (0x08 = OFF, 0x0F = ON)
initialization_list = [0x3A, 0x09, 0x06, 0x1E, 0x39, 0x1B, 0x6E, 0x56, 0x7A, 0x38, 0x0F]

# add 1 to the display reg address to set the write bit high
bus.write_i2c_block_data(display_register_address+1, 0x00, initialization_list)

# Create a dictionary to easily output words on the display
Alpha = {"A": 0x41, "B": 0x42, "C": 0x43, "D": 0x44, "E": 0x45, "F": 0x46, "G": 0x47, "H":0x48, "I": 0x49, "J": 0x4A,
         "K": 0x4B, "L": 0x4C, "M": 0x4D, "N": 0x4E, "O": 0x4F, "P": 0x50, "Q": 0x51, "R": 0x52, "S":0x53, "T": 0x54,
         "U": 0x55, "V": 0x56, "W": 0x57, "X": 0x58, "Y": 0x59, "Z": 0x5A, "Ohm": 0xB5, ":": 0x3A, "-": 0x2D}

Num = {"0": 0x30, "1": 0x31, "2": 0x32, "3": 0x33, "4": 0x34, "5": 0x35, "6": 0x36, "7": 0x37, "8": 0x38, "9": 0x39,
       ".": 0x2E}

# select ROM_A for the display
# reg values: RE high, ROM selection, ROM choice (0x00 = ROM_A, 0x04 = ROM_B, 0x08 = ROM_C)
# note that 0x40 or 0x5F need to be written in order to actually select the ROM, bits are 10XX XXXX
bus.write_i2c_block_data(display_register_address+1, 0x00, [0x3A])
bus.write_i2c_block_data(display_register_address+1, 0x00, [0x72])
bus.write_i2c_block_data(display_register_address+1, 0x40, [0x00])
bus.write_i2c_block_data(display_register_address+1, 0x00, [0x38])

# set display to double height mode
bus.write_i2c_block_data(display_register_address+1,0x00,[0x3A, 0x1B, 0x3C])

# clear display
bus.write_i2c_block_data(display_register_address+1, 0x00, [0x01])

direction = "clockwise"  # set direction global variable

# The motor used has 200 steps per full revolution, which is 1.8 degrees per step
measured_step = 0  # variable used to show position on LCD display

# set ADC variables
VDD = 3.3  # VDD and full scale voltage Of ADC
ADC_Bits = 12  # the ADC121C021 is a 12-bit ADC
ADC_full_scale = (2**ADC_Bits)-1  # 2^12 States for the ADC output, but indexing starts at 0, so the variable is
# decremented by 1

# ADC1 is used for the set value and reads in the voltage controlled by the potentiometer knob
ADC_1_device_address = 0x50 # the device address of the ADC121C021, which is assigned as a hexadecimal number
ADC_1_result_register_address = 0x0
final_read_result_1 = ""  # initialize a blank string that will form the final read result

# ADC2 is used for positional feedback and reads the voltage on the dynamic resistive divider to provide current
# location information
ADC_2_device_address = 0x51
ADC_2_result_register_address = 0x0
final_read_result_2 = ""

# create a list of 0 ohm values that corresponds to the length of the track where a short exists
R_set0 = [0 for i in range(1, 7)]

# there are 30x 0.050 Ohm resistors spaced ~two steps from each other
# this list comprehension creates a list of sequenced resistances that matches the actual design of the motorized load
# and is interleaved with itself to double its size
R_set1 = [round(i * 0.05, 2) for i in range(0, 31)]
R_set1 = [value for pair in zip(R_set1, R_set1) for value in pair]

# there are 30x 0.10 Ohm resistors spaced ~two steps from each other
# this list comprehension creates a list of sequenced resistances that matches the actual design of the motorized load
# and is interleaved with itself to double its size
R_set2 = [round((i + 1) * 0.10 + R_set1[-1], 2) for i in range(0, 30)]
R_set2 = [value for pair in zip(R_set2, R_set2) for value in pair]

# there are 24x 1.5 Ohm resistors spaced ~three steps from each other
# this list comprehension creates a list of sequenced resistances that matches the
# motorized load and is interleaved with itself to double its size
R_set3 = [round((i + 1) * 1.5 + R_set2[-1], 2) for i in range(0, 24)]
R_set3 = [value for pair in zip(R_set3, R_set3, R_set3) for value in pair]

# merge the interleaved lists together to create a "set" array of resistors,
# the values of which correspond to the steps that the motor can target
R_set = R_set0 + R_set1 + R_set2 + R_set3

# create a list of feedback voltages that correspond to different loads
V_fb = list()

# initialize a list to store the current state of the motor's logic inputs
state_list = [GPIO.HIGH, GPIO.HIGH, GPIO.LOW, GPIO.LOW]


def RotateOneStepClockwise(state_list_param):
    # define an interrupt/callback that accepts the global variable "state_list" to single rotate the motor,
    # by a single clockwise step, based on the current state of the motor driver logic inputs
    
    global Delay  # allow the callback to access the global "Delay" variable
    global VDD
    global ADC_Bits
    global ADC_full_scale
    global ADC_1_device_address
    global ADC_1_result_register_address
    global final_read_result_1
    global ADC_2_device_address
    global ADC_2_result_register_address
    global final_read_result_2
    
    # initialize the new state list variable "new_state_list"
    new_state_list = [None, None, None, None]
    
    # set the new state, based on the current state, such that the motor rotates by a single clockwise step
    new_state_list[0] = state_list[3]
    new_state_list[1] = state_list[0]
    new_state_list[2] = state_list[1]
    new_state_list[3] = state_list[2]

    # set the Raspberry Pi digital outputs (motor drive logic inputs) according to the newly determined state,
    # which is based on the current state
    GPIO.output(6, state_list[0])
    GPIO.output(19, state_list[1])
    GPIO.output(13, state_list[2])
    GPIO.output(26, state_list[3])
    
    tm.sleep(Delay)  # delay between steps according to the global "Delay" variable

    # read in two bytes (16 bits) of data
    ADC_2_read_result = bus.read_i2c_block_data(ADC_2_device_address, ADC_2_result_register_address, 2)
    
    # concatenate results to form final read result
    # the first byte returned is shifted by 8 digits before concatenating results
    final_read_result_2 = ((ADC_2_read_result[0]) << 8) | (ADC_2_read_result[1])

    # for debug
    # final_read_voltage/VDD = final_read_result/ADC_full_scale -> solve for final_read_voltage
    # current_read_voltage = VDD*(final_read_result)/ADC_full_scale -> convert the reading into a voltage
    
    return new_state_list  # return the new state of the motor drive inputs

def RotateOneStepCounterClockwise(state_list_param):
    # define an interrupt/callback that accepts the global variable "state_list" to single rotate the motor,
    # by a single counterclockwise step, based on the current state of the motor driver logic inputs
    
    global Delay  # allow the callback to access the global "Delay" variable
    global VDD
    global ADC_Bits
    global ADC_full_scale
    global ADC_1_device_address
    global ADC_1_result_register_address
    global final_read_result_1
    global ADC_2_device_address
    global ADC_2_result_register_address
    global final_read_result_2
    
    # initialize the new state list variable "new_state_list"
    new_state_list = [None, None, None, None]
    
    # set the new state, based on the current state, such that the motor rotates by a single step
    new_state_list[0] = state_list[1]
    new_state_list[1] = state_list[2]
    new_state_list[2] = state_list[3]
    new_state_list[3] = state_list[0]

    # set the Raspberry Pi digital outputs (motor drive logic inputs) according to the newly determined state,
    # which is based on the current state
    GPIO.output(6, state_list[0])
    GPIO.output(19, state_list[1])
    GPIO.output(13, state_list[2])
    GPIO.output(26, state_list[3])
    
    tm.sleep(Delay)  # delay between steps according to the global "Delay" variable
    
    # read in two bytes (16 bits) of data
    ADC_2_read_result = bus.read_i2c_block_data(ADC_2_device_address,ADC_2_result_register_address,2)

    # concatenate results to form final read result
    # the first byte returned is shifted by 8 digits before concatenating results
    final_read_result_2 = ((ADC_2_read_result[0]) << 8) | (ADC_2_read_result[1])

    # for debug
    # final_read_voltage/VDD = final_read_result/ADC_full_scale -> solve for final_read_voltage
    # current_read_voltage = VDD*(final_read_result)/ADC_full_scale -> convert the reading into a voltage
 
    return new_state_list  # return the new state of the motor drive inputs

def MotorStepInterrupt(self):
    # define an interrupt/callback that accesses a number of global variables and controls the motor movement to move it
    # from its current position to the desired position, such that the target resistance is formed between its terminals
    # 
    # The callback/interrupt wakes up the motor drive chip. It then determines the direction of rotation and enters a
    # while loop. After entering the loop, it will call either the "RotateOneStepClockwise" function or the
    # "RotateOneStepCounterClockwise" function depending on the "direction" variable value. The loop exits once the
    # final voltage target obtained via the first ADC matches the voltage, within an acceptable tolerance, on the second
    # ADC. The current state of the motor drive logic inputs are tracked and updated with each iteration in the while
    # loop.
    
    # declare global variables that the callback/interrupt will need access to
    global direction
    global measured_step
    global state_list
    global VDD
    global ADC_Bits
    global ADC_full_scale
    global ADC_1_device_address
    global ADC_1_result_register_address
    global final_read_result_1
    global ADC_2_device_address
    global ADC_2_result_register_address
    global final_read_result_2
    global R_set
      
    # read in two bytes (16 bits) of data
    ADC_1_read_result = bus.read_i2c_block_data(ADC_1_device_address, ADC_1_result_register_address, 2) 
    
    # concatenate results to form final read result
    # the first byte returned is shifted by 8 digits before concatenating results
    final_read_result_1 = ((ADC_1_read_result[0]) << 8) | (ADC_1_read_result[1])
    
    # convert the ADC reading into a target voltage
    final_read_voltage = VDD*final_read_result_1/ADC_full_scale 
    print('ADC Reading = '+str(final_read_voltage) + ' V')

    # read in two bytes (16 bits) of data
    ADC_2_read_result = bus.read_i2c_block_data(ADC_2_device_address, ADC_2_result_register_address, 2) 
    
    # concatenate results to form final read result
    # the first byte returned is shifted by 8 digits before concatenating results
    final_read_result_2 = ((ADC_2_read_result[0]) << 8) | (ADC_2_read_result[1])

    # convert the ADC reading into a voltage that represents current position
    current_read_voltage = VDD*final_read_result_2/ADC_full_scale 

    # select the target resistance based on the knob (potentiometer) voltage reading
    R_target_index = round((final_read_result_1/ADC_full_scale)*len(R_set))
    
    # do not allow the motor to turn more than a full rotation to get to its target
    if R_target_index > 199:
        R_target_index = 199
    
    # select the target resistance which is equal to the desired load resistance
    R_target = R_set[R_target_index]
    
    # choose a threshold that defines the range for an acceptable reading - this changes based on resistor values used
    # these values were verified empirically for this specific design
    if R_target < 1.5:
        tolerance = 0.001
    elif R_target > 4.5:
        tolerance = 0.030
    else:
        tolerance = 0.001
    
    # calculate Vdiv, the expected voltage at the position of interest
    final_voltage_target = ((3.3/(49.9+40.5-R_target))*(40.5-R_target))
    
    # determine direction of rotation based on the current position and target resistance
    if final_voltage_target > current_read_voltage:
        direction = "counter_clockwise"
    else:
        direction = "clockwise"

    i = 0  # reset index
    while 1:
        Delay = 10/1000  # sets an additional delay between rotations
        current_read_voltage = VDD*final_read_result_2/ADC_full_scale
        if direction == "clockwise":
            state_list = RotateOneStepClockwise(state_list)  # clockwise rotation
        else:
            state_list = RotateOneStepCounterClockwise(state_list)  # counterclockwise rotation
        i = i+1  # increment the counter after either function is called

        if (abs(final_voltage_target - current_read_voltage) < tolerance) or (i > 199):  # while loop exit condition
            measured_step = R_target_index
            print("R_target: " + str(R_target))
            print("final_voltage_target: " + str(final_voltage_target))
            print("current_read_voltage: " + str(current_read_voltage))
            break

    # additional delay
    tm.sleep(Delay)


# main code

GPIO.setwarnings(False)  # disable warnings that occur when configuring the GPIO signals

# refer to the GPIO signals based on the broadcom chip number and not the board's breakout header containing the pins
GPIO.setmode(GPIO.BCM)

GPIO.setup(1, GPIO.IN, GPIO.PUD_UP) # set GPIO 1 as a digital input pin and have it pulled up by default

# configure a trigger event for the callback/interrupt "MotorStepInterrupt". The trigger is set to occur when a digital
# falling edge occurs on GPIO 5 and a debounce time of 3000ms is set following an event
GPIO.add_event_detect(1, GPIO.FALLING, callback=MotorStepInterrupt, bouncetime=3000)
 
# enable/wakeup the motor drive IC
GPIO.setup(0, GPIO.OUT)
GPIO.output(0, GPIO.HIGH)

# set all of the GPIO signals that will be designated as logic inputs to the motor drive to be digital outputs on the
# Raspberry Pi
GPIO.setup(6, GPIO.OUT)
GPIO.setup(19, GPIO.OUT)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(26, GPIO.OUT)

# set all of the digital inputs to the motor drive to logic low initially
GPIO.output(6, GPIO.LOW)
GPIO.output(19, GPIO.LOW)
GPIO.output(13, GPIO.LOW)
GPIO.output(26, GPIO.LOW)

Delay = 10  # Delay in s
Delay = Delay/1000  # Delay in ms

while 1:
    
    # setting all of the  the Raspberry Pi outputs (motor drive logic inputs) has almost the same effect
    # as disabling the motor drive IC. This reduces the consumed power while the motor isn't in motion,
    # but doesn't protect the motor from unwanted rotation due to any potential external torque sources
    GPIO.output(6, GPIO.LOW)
    GPIO.output(19, GPIO.LOW)
    GPIO.output(13, GPIO.LOW)
    GPIO.output(26, GPIO.LOW)
    
    # read in two bytes (16 bits) of data
    ADC_1_read_result = bus.read_i2c_block_data(ADC_1_device_address, ADC_1_result_register_address, 2)

    # concatenate results to form final read result
    # the first byte returned is shifted by 8 digits before concatenating results
    final_read_result_1 = ((ADC_1_read_result[0]) << 8) | (ADC_1_read_result[1])
    final_read_voltage = VDD*final_read_result_1/ADC_full_scale

    # user's current resistor selection to be displayed on LCD display
    current_step = round(final_read_result_1*199/ADC_full_scale)

    # prepare strings to be displayed on LCD display
    set_string = "MEAS:"+str(R_set[round(measured_step)])
    meas_string = "SET:"+str(R_set[round(current_step)])
    set_string_list = list()
    meas_string_list = list()

    for character in set_string:
        try:
            set_string_list.append(Alpha[character])
        except:
            set_string_list.append(Num[character])
        
    for character in meas_string:
        try:
            meas_string_list.append(Alpha[character])
        except:
            meas_string_list.append(Num[character])
            
    set_string_list.append(Alpha["Ohm"])
    meas_string_list.append(Alpha["Ohm"])
    
    # clear display 
    bus.write_i2c_block_data(display_register_address+1, 0x00, [0x01])

    # display resistor value to be set (SET) on LCD display
    bus.write_i2c_block_data(display_register_address+1, 0x40, set_string_list)
    
    # move cursor position to 1st position on 1st row
    bus.write_i2c_block_data(display_register_address+1, 0x00, [0xA0])

    # display current resistance (MEAS) on LCD display
    bus.write_i2c_block_data(display_register_address+1, 0x40, meas_string_list)
        
    # add a delay of at least half a second between setting the GPIO low and then setting it high.
    tm.sleep(500*1/1000)

# close I2C communication bug - code only reachable while debugging
# bus.close() # close communication line of the bus object