'''Control script for SNAPpy powered OWI Robotic Arm Edge

   Base platform is SNAP Protoboard.
'''

from synapse.platforms import *
from synapse.pinWakeup import *

# Proto board pin definitions
PROTO_LED_GRN_PIN = GPIO_1
PROTO_LED_YLW_PIN = GPIO_2
PROTO_BUTTON_PIN  = GPIO_5

# Robot Arm pin definitions: OWI 535-USB board, modified for SNAP
GRIP1 = U2_IN1 = U1P6 = GPIO_15
GRIP2 = U2_IN2 = U1P2 = GPIO_11

WRIST1 = U2_IN3 = U1P3 = GPIO_12
WRIST2 = U2_IN4 = U1P4 = GPIO_13

LAMP = U1P5 = GPIO_14

ELBOW1 = U3_IN1 = U1P23 = GPIO_16
ELBOW2 = U3_IN2 = U1P24 = GPIO_17

SHOULDER1 = U3_IN3 = U1P22 = GPIO_4
SHOULDER2 = U3_IN4 = U1P21 = GPIO_3

BASE1 = U4_IN1 = U1P18 = GPIO_10
BASE2 = U4_IN2 = U1P7 = GPIO_9

# Hold pushbutton for sleep
buttonDownCount = 20

# Limit motor pulse duration for safety
MAX_PULSE = 10  # 1sec

# Motor pulse timeout values (0.1sec intervals, sign determines direction, zero is off)
tGrip = 0
tWrist = 0
tElbow = 0
tShoulder = 0
tBase = 0

@setHook(HOOK_STARTUP)
def start():
    
    # Set all pins to output low
    i = 0
    while i < len(GPIO_TO_IO_LIST):
        setPinDir(GPIO_TO_IO_LIST[i], True)
        writePin(GPIO_TO_IO_LIST[i], False)
        i += 1

    # Revert button and UART RX to inputs
    setPinDir(GPIO_5, False)
    setPinDir(GPIO_7, False)
    
@setHook(HOOK_100MS)
def tick100ms():
    '''Blink LEDs, check pushbutton, and update motor control. Go to sleep if button held; wake on press.'''
    global buttonDownCount
    
    if readPin(PROTO_BUTTON_PIN):
        # Not pressed
        buttonDownCount = 20  # 2sec hold required
        pulsePin(PROTO_LED_GRN_PIN, 20, True)
    else:
        buttonDownCount -= 1
        if buttonDownCount == 0:
            wakeupOn(PROTO_BUTTON_PIN, True, False)
            allStop()
            while buttonDownCount < 20:
                sleep(0, 0)   # Sleep until pin wakeup
                
                # Debounce wakeup press
                buttonDownCount = 0
                while not readPin(PROTO_BUTTON_PIN):
                    buttonDownCount += 1
            
            
        pulsePin(PROTO_LED_YLW_PIN, 60, True)
        
    motorTick()
    
def lampOn():
    robotPulse(None, None, None, None, None, True)
def lampOff():
    robotPulse(None, None, None, None, None, False)
def gripOpen():
    robotPulse(MAX_PULSE, None, None, None, None, None)
def gripClose():
    robotPulse(-MAX_PULSE, None, None, None, None, None)
def wristUp():
    robotPulse(None, MAX_PULSE, None, None, None, None)
def wristDown():
    robotPulse(None, -MAX_PULSE, None, None, None, None)
def elbowUp():
    robotPulse(None, None, MAX_PULSE, None, None, None)
def elbowDown():
    robotPulse(None, None, -MAX_PULSE, None, None, None)
def shoulderUp():
    robotPulse(None, None, None, MAX_PULSE, None, None)
def shoulderDown():
    robotPulse(None, None, None, -MAX_PULSE, None, None)
def baseCW():
    robotPulse(None, None, None, None, MAX_PULSE, None)
def baseCCW():
    robotPulse(None, None, None, None, -MAX_PULSE, None)
def allStop():
    robotPulse(0,0,0,0,0,0)
    

def decay(val):
    '''Values decay to zero over time'''
    if val > 0:
        val -= 1
    elif val < 0:
        val += 1
    return val

def motorTick():
    '''Every 100ms, decay motor timers'''
    global tGrip, tWrist, tElbow, tShoulder, tBase
    tGrip = decay(tGrip)
    tWrist = decay(tWrist)
    tElbow = decay(tElbow)
    tShoulder = decay(tShoulder)
    tBase = decay(tBase)
    engageMotors()
    
# Motor control H-bridge pin groupings
CMD_IN1 = (GRIP1, WRIST1, ELBOW1, SHOULDER1, BASE1, LAMP)
CMD_IN2 = (GRIP2, WRIST2, ELBOW2, SHOULDER2, BASE2, LAMP)

def engageMotors():
    '''Control motors per global timer values'''
    motorControl(0, tGrip)
    motorControl(1, -tWrist)
    motorControl(2, tElbow)
    motorControl(3, tShoulder)
    motorControl(4, tBase)

def motorControl(idx, val):
    '''Control indexed motor H-bridge, based on timer val'''
    if val > 0:
        writePin(CMD_IN1[idx], False)
        writePin(CMD_IN2[idx], True)
    elif val < 0:
        writePin(CMD_IN2[idx], False)
        writePin(CMD_IN1[idx], True)
    else:
        writePin(CMD_IN1[idx], False)
        writePin(CMD_IN2[idx], False)

def newPulseVal(old, requested):
    '''Return range-limited "new value" for motor timer'''
    if requested is not None and (-MAX_PULSE <= requested <= MAX_PULSE):
        return requested
    return old

def robotPulse(grip, wrist, elbow, shoulder, base, lamp):
    '''Safe means of commanding robot. Pulse values are +-0.1s or None.'''
    global tGrip, tWrist, tElbow, tShoulder, tBase
    tGrip = newPulseVal(tGrip, grip)
    tWrist = newPulseVal(tWrist, wrist)
    tElbow = newPulseVal(tElbow, elbow)
    tShoulder = newPulseVal(tShoulder, shoulder)
    tBase = newPulseVal(tBase, base)
    engageMotors()
    if lamp is not None:
        writePin(LAMP, lamp)

