Adding more servos for sensors

So I can now reset the robot arm using a for loop, and separately move shoulder, elbow, wrist, and hand using one SetPwmDutyCycle() statement.


    for i in range(len(robotArmStateVector)):
SetPwmDutyCycle(spiGuzuntyPi, i, robotArmStateVector[i])

    SetPwmDutyCycle(spiGuzuntyPi, Shoulder, Middle)


    SetPwmDutyCycle(spiGuzuntyPi, Elbow, LeftMost)

    SetPwmDutyCycle(spiGuzuntyPi, Wrist, LeftMost)

    SetPwmDutyCycle(spiGuzuntyPi, Hand, RightMost)


The robot arm for now uses 4 servos.  So there are 4 PWM channels left over.  Perhaps I can now add more body parts, like Infrared detector as eye, ultrasonic sensor as ear, as PID as sixth sensor, ...




def MoveGuzuntyPiRobotArm():

    PrintDoubleSpaceLine("*** Start testing Guzunty Pi Robot Arm ***")   

    # Open SPI Channel 0,0 for Guzunty Pi

    spiGuzuntyPi = spidev.SpiDev() 
    spiGuzuntyPi.open(0, 0)  

    # * PWM Duty Cycle assignment *

    CwMax = 13
    CcwMax = 30
    RightMost = CwMax
    LeftMost = CcwMax
    Middle = (RightMost + LeftMost) / 2

    # * Enum functions for servo numbers *

    Shoulder = 0
    Elbow = 1
    Wrist = 2
    Hand = 3

    # * Robot arm state vector and tuple *

    robotArmStateVector = [Middle, Middle, Middle, Middle, Middle, Middle, Middle, Middle]

    RobotArmStateTupleAllMiddle = Middle, Middle, Middle, Middle, Middle, Middle, Middle, Middle
    RobotArmStateTupleAllRightMost = RightMost, RightMost, RightMost, RightMost, RightMost, RightMost, RightMost, RightMost
    RobotArmStateTupleAllLeftMost = LeftMost, LeftMost, LeftMost, LeftMost, LeftMost, LeftMost, LeftMost, LeftMost

    RobotArmStateTupleReset = RightMost, Middle, RightMost, LeftMost, Middle, Middle, Middle, Middle

    # * Transfer robot arm state tuple values to state vector

    for i in range(len(RobotArmStateTupleAllMiddle)):
        #robotArmStateVector[i] = RobotArmStateTupleAllLeftMost[i]
        robotArmStateVector[i] = RobotArmStateTupleReset[i]

    for i in range(len(robotArmStateVector)):
SetPwmDutyCycle(spiGuzuntyPi, i, robotArmStateVector[i])
    time.sleep(2)

    Beep(2)
    SetPwmDutyCycle(spiGuzuntyPi, Shoulder, Middle)
    time.sleep(2)

    Beep(2)
    SetPwmDutyCycle(spiGuzuntyPi, Elbow, LeftMost)
    time.sleep(2)

    Beep(2)
    SetPwmDutyCycle(spiGuzuntyPi, Wrist, LeftMost)
    time.sleep(2)

    Beep(2)
    SetPwmDutyCycle(spiGuzuntyPi, Hand, RightMost)
    time.sleep(2)

    spiGuzuntyPi.close() 

    PrintDoubleSpaceLine("*** Stop testing Guzunty Pi ***") 

.END