1#!/bin/bash -e
2
3# Initialize for step motor of sled:
4#   Enable pwm and setup pwm duty
5#   Setup gpio pins for step motor control
6#   Moving step motor back to initial position
7
8export PATH=$PATH:/usr/libexec
9
10PWM_CLASS_PATH="/sys/class/pwm/pwmchip0"
11#Sleld 1~6 using bmc pwm8~13 as motor driver stick
12PWM_NUM_OFFSET=7
13PWM_PERIOD=2500000 #400HZ
14PWM_DUTY=250000    #PWM_PERIOD X 10%
15CALIBRATE_TIMEOUT=120
16
17#Enable pwm for sledN
18function open_pwm() {
19    local SLED_NUM="$1"
20    echo "Open pwm of sled$SLED_NUM"
21    PWM_NUM=$(( SLED_NUM + PWM_NUM_OFFSET ))
22    PWM_PATH="${PWM_CLASS_PATH}/pwm${PWM_NUM}"
23    if [ ! -d "$PWM_PATH" ];then
24        echo "$PWM_NUM" > "${PWM_CLASS_PATH}/export"
25    fi
26    if [ -d "$PWM_PATH" ];then
27        echo "set pwm period to $PWM_PERIOD ns"
28        if ! echo "$PWM_PERIOD" > "${PWM_PATH}/period"; then
29            echo "Error: set pwm period fail"
30            return 1
31        fi
32
33        if ! echo 1 > "${PWM_PATH}/enable"; then
34            echo "Error: set pwm enable fail"
35            return 1
36        fi
37
38        if ! echo "$PWM_DUTY" > "${PWM_PATH}/duty_cycle"; then
39            echo "Error: set pwm duty_cycle fail"
40            return 1
41        fi
42    else
43        echo "Error: ${PWM_PATH} not exist, export pwm${PWM_NUM} fail"
44        return 1
45    fi
46}
47
48function set_gpio()
49{
50    NET_NAME=$1
51    OUT_VAL=$2
52    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
53    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
54        echo "set_gpio: can not find gpio, $NET_NAME"
55        return 1
56    fi
57    echo -n "set_gpio: set $NET_NAME = $OUT_VAL"
58    if ! gpioset "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}"="$OUT_VAL"; then
59        echo " failed"
60        return 1
61    fi
62    echo " success"
63    return 0
64}
65
66function get_gpio()
67{
68    NET_NAME=$1
69    RET_VAL=2
70
71    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
72    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
73        echo "get_gpio: can not find gpio, $NET_NAME" >&2
74        return 1
75    fi
76    if ! RET_VAL=$(gpioget "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}") ; then
77        echo "get_gpio: get ${NET_NAME} failed" >&2
78        return 1
79    fi
80    echo "${RET_VAL}"
81    return 0
82}
83
84#Init gpio pins for step motor control
85function init_gpios() {
86    echo "Init GPIOs:"
87    motor_ctrl_gpio_pins_names=(    "SLED${1}_MD_STBY_RESET"
88                                    "SLED${1}_MD_IOEXP_EN_FAULT"
89                                    "SLED${1}_MD_DIR"
90                                    "SLED${1}_MD_DECAY"
91                                    "SLED${1}_MD_MODE1"
92                                    "SLED${1}_MD_MODE2"
93                                    "SLED${1}_MD_MODE3" )
94
95    for  gpio_name in "${motor_ctrl_gpio_pins_names[@]}"; do
96        set_gpio "$gpio_name"   0
97    done
98}
99
100if [[ "$1" =~ ^(sled[1-6]{1})$ ]]; then
101  SLED=$1
102  SLED_NUM=${SLED:4}
103else
104  #show_usage
105  echo "invalid sled name: ${1}"
106  exit 1;
107fi
108
109#Check if sled is present
110SLED_PRESENT=$(get_gpio "presence-sled${SLED_NUM}")
111if [ "$SLED_PRESENT" != 0 ];then
112    echo "${SLED} is not present, skip motor initialize"
113    exit 1
114fi
115
116#Init gpios
117init_gpios "$SLED_NUM"
118
119#enable pwm
120open_pwm "$SLED_NUM"
121
122#SLED{N}_MS_DETECT1  (initial position)
123DETECT_PIN1="SLED${SLED_NUM}_MS_DETECT1"
124INIT_POS=$(get_gpio "$DETECT_PIN1")
125
126if [ "$INIT_POS" -eq 1 ];then
127    time_count=0
128    echo "Making motor back to initial position..."
129    motor-ctrl "$SLED" r >/dev/null
130    while  [ "$INIT_POS" -eq 1 ]  ;do
131        INIT_POS=$(get_gpio "$DETECT_PIN1")
132        sleep 0.1
133        time_count=$(( time_count + 1 ))
134        if [  $time_count -gt $CALIBRATE_TIMEOUT ];then
135            echo "Error: Step motor run over 1 cycle but switch never triggered"
136            break
137        fi
138    done
139    motor-ctrl "$SLED" s >/dev/null
140fi
141
142if [ "$INIT_POS" -eq 0 ];then
143    echo "Motor calibrated to initial position."
144    exit 0
145else
146    echo "Find motor initial position failed"
147    exit 1
148fi
149