Initial commit

meta-facebook/meta-bletchley/recipes-bletchley/motor-ctrl/files/motor-init

@@ -0,0 +1,107 @@
+#!/bin/bash -e
+
+# Initialize for step motor of sled:
+#   Enable pwm and setup pwm duty
+#   Setup gpio pins for step motor control
+#   Moving step motor back to initial position
+
+export PATH=$PATH:/usr/libexec
+
+# shellcheck source=meta-facebook/meta-bletchley/recipes-bletchley/plat-tools/files/bletchley-common-functions
+source /usr/libexec/bletchley-common-functions
+
+PWM_CLASS_PATH="/sys/class/pwm/pwmchip0"
+#Sleld 1~6 using bmc pwm8~13 as motor driver stick
+PWM_NUM_OFFSET=7
+PWM_PERIOD=2500000 #400HZ
+PWM_DUTY=250000    #PWM_PERIOD X 10%
+CALIBRATE_TIMEOUT=120
+
+#Enable pwm for sledN
+function open_pwm() {
+    local SLED_NUM="$1"
+    echo "Open pwm of sled$SLED_NUM"
+    PWM_NUM=$(( SLED_NUM + PWM_NUM_OFFSET ))
+    PWM_PATH="${PWM_CLASS_PATH}/pwm${PWM_NUM}"
+    if [ ! -d "$PWM_PATH" ];then
+        echo "$PWM_NUM" > "${PWM_CLASS_PATH}/export"
+    fi
+    if [ -d "$PWM_PATH" ];then
+        echo "set pwm period to $PWM_PERIOD ns"
+        if ! echo "$PWM_PERIOD" > "${PWM_PATH}/period"; then
+            echo "Error: set pwm period fail"
+            return 1
+        fi
+
+        if ! echo 1 > "${PWM_PATH}/enable"; then
+            echo "Error: set pwm enable fail"
+            return 1
+        fi
+
+        if ! echo "$PWM_DUTY" > "${PWM_PATH}/duty_cycle"; then
+            echo "Error: set pwm duty_cycle fail"
+            return 1
+        fi
+    else
+        echo "Error: ${PWM_PATH} not exist, export pwm${PWM_NUM} fail"
+        return 1
+    fi
+}
+
+#Init gpio pins for step motor control
+function init_gpios() {
+    echo "Init GPIOs:"
+    motor_ctrl_gpio_pins_names=(    "SLED${1}_MD_STBY_RESET"
+                                    "SLED${1}_MD_IOEXP_EN_FAULT"
+                                    "SLED${1}_MD_DIR"
+                                    "SLED${1}_MD_DECAY"
+                                    "SLED${1}_MD_MODE1"
+                                    "SLED${1}_MD_MODE2"
+                                    "SLED${1}_MD_MODE3" )
+
+    for  gpio_name in "${motor_ctrl_gpio_pins_names[@]}"; do
+        set_gpio "$gpio_name"   0
+    done
+}
+
+if [[ "$1" =~ ^(sled[1-6]{1})$ ]]; then
+  SLED=$1
+  SLED_NUM=${SLED:4}
+else
+  #show_usage
+  echo "invalid sled name: ${1}"
+  exit 1;
+fi
+
+#Check if sled is present
+if ! is_sled_present "${SLED_NUM}"; then
+    echo "${SLED} is not present, skip motor initialize"
+    exit 1
+fi
+
+#Init gpios
+init_gpios "$SLED_NUM"
+
+#enable pwm
+open_pwm "$SLED_NUM"
+
+#SLED{N}_MS_DETECT1  (initial position)
+DETECT_PIN1="SLED${SLED_NUM}_MS_DETECT1"
+INIT_POS=$(get_gpio "$DETECT_PIN1")
+
+if [ "$INIT_POS" -eq 1 ];then
+    echo "Making motor back to initial position..."
+    motor-ctrl "$SLED" r >/dev/null
+    wait_gpio_falling "$DETECT_PIN1" "$CALIBRATE_TIMEOUT"
+    motor-ctrl "$SLED" s >/dev/null
+fi
+
+INIT_POS=$(get_gpio "$DETECT_PIN1")
+if [ "$INIT_POS" -eq 0 ];then
+    echo "Motor calibrated to initial position."
+    exit 0
+else
+    echo "Error: Step motor run over 1 cycle but switch never triggered"
+    echo "Find motor initial position failed"
+    exit 1
+fi