1 files changed, 101 insertions, 0 deletions
diff --git a/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-ctrl b/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-ctrl
new file mode 100755
index 000000000..3f8f2ca21
--- /dev/null
+++ b/meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-ctrl
@@ -0,0 +1,101 @@
+#!/bin/bash -e
+#
+# Control step motor rotate of sled
+
+function set_gpio()
+{
+    NET_NAME=$1
+    OUT_VAL=$2
+    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
+    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
+        echo "set_gpio: can not find gpio, $NET_NAME"
+        return 1
+    fi
+    echo -n "set_gpio: set $NET_NAME = $OUT_VAL"
+    if ! gpioset "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}"="$OUT_VAL"; then
+        echo " failed"
+        return 1
+    fi
+    echo " success"
+    return 0
+}
+
+#Get i2c bus number for sledN
+function get_bus_num() {
+    SLED_NUM=$1
+    local bus=0
+
+    if [[ "$SLED_NUM" = [0-5] ]]; then
+        bus="$SLED_NUM"
+    fi
+    echo "$bus"
+}
+
+#Enable sledN Motor VRef
+function open_vref() {
+    i2cset -f -y "${1}" 0x67 0x06 0x95
+}
+
+#Disable sledN Motor VRef
+function close_vref() {
+    i2cset -f -y "${1}" 0x67 0x06 0x55
+}
+
+#######################################
+# Setting step motor control pins to start/stop motor
+# Arguments:
+#   1. SLED NUMBER
+#   2. Value of STBY RESET PIN
+#   3. Value of ENABLE PIN
+#   4. Value of DIRECTION PIN
+#######################################
+function set_motor() {
+    STBY_PIN="SLED${1}_MD_STBY_RESET"
+    EN_PIN="SLED${1}_MD_IOEXP_EN_FAULT"
+    DIR_PIN="SLED${1}_MD_DIR"
+    set_gpio "$STBY_PIN" "$2"
+    set_gpio "$EN_PIN"   "$3"
+    set_gpio "$DIR_PIN"  "$4"
+}
+
+function show_usage(){
+    echo "Usage: motor-ctrl [sled0 | sled1 | sled2 | sled3 | sled4 | sled5]  [f r s]"
+    echo "      f : Step Motor go forward"
+    echo "      r : Step Motor go reverse"
+    echo "      s : Step Motor stop "
+}
+
+if [ $# -ne 2 ]; then
+    show_usage
+    exit 1;
+fi
+
+if [[ "$1" =~ ^(slot[0-5]{1})$ ]] || [[ "$1" =~ ^(sled[0-5]{1})$ ]]; then
+    SLED=$1
+    SLED_NUM=${SLED:4}
+    I2C_NUM=$(get_bus_num "$SLED_NUM")
+    ACTION=$2
+else
+     echo "invalid sled name: $1"
+    exit 1;
+fi
+
+if [[ "$ACTION" == "s" ]]; then
+    echo "stop motor"
+    set_motor "$SLED_NUM" 1 0 0
+    close_vref "$I2C_NUM"
+elif [[ "$ACTION" == "f" ]];then
+    echo "start motor, direction:forward"
+    set_motor  "$SLED_NUM" 1 1 0
+    open_vref "$I2C_NUM"
+elif [[ "$ACTION" == "r" ]];then
+    echo "start motor, direction:reverse"
+    set_motor "$SLED_NUM" 1 1 1
+    open_vref "$I2C_NUM"
+else
+    echo "Error: Unknown action!"
+    exit 1
+fi
+
+exit 0
+