Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 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 | |
| 8 | export PATH=$PATH:/usr/libexec |
| 9 | |
Allen.Wang | 6af0dff | 2021-12-28 20:23:05 +0800 | [diff] [blame] | 10 | PWM_CLASS_PATH="/sys/class/pwm/pwmchip0" |
Allen.Wang | 5ff992e | 2022-01-04 21:00:38 +0800 | [diff] [blame] | 11 | #Sleld 1~6 using bmc pwm8~13 as motor driver stick |
| 12 | PWM_NUM_OFFSET=7 |
Allen.Wang | 6af0dff | 2021-12-28 20:23:05 +0800 | [diff] [blame] | 13 | PWM_PERIOD=2500000 #400HZ |
| 14 | PWM_DUTY=250000 #PWM_PERIOD X 10% |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 15 | CALIBRATE_TIMEOUT=120 |
| 16 | |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 17 | #Enable pwm for sledN |
| 18 | function open_pwm() { |
| 19 | local SLED_NUM="$1" |
| 20 | echo "Open pwm of sled$SLED_NUM" |
Allen.Wang | 6af0dff | 2021-12-28 20:23:05 +0800 | [diff] [blame] | 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" |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 25 | fi |
Allen.Wang | 6af0dff | 2021-12-28 20:23:05 +0800 | [diff] [blame] | 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 |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 32 | |
Allen.Wang | 6af0dff | 2021-12-28 20:23:05 +0800 | [diff] [blame] | 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 |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 46 | } |
| 47 | |
| 48 | function 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 | |
| 66 | function 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 |
| 85 | function 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 | |
Allen.Wang | 5ff992e | 2022-01-04 21:00:38 +0800 | [diff] [blame] | 100 | if [[ "$1" =~ ^(sled[1-6]{1})$ ]]; then |
Allen.Wang | 4a0948d | 2021-12-15 13:41:18 +0800 | [diff] [blame] | 101 | SLED=$1 |
| 102 | SLED_NUM=${SLED:4} |
| 103 | else |
| 104 | #show_usage |
| 105 | echo "invalid sled name: ${1}" |
| 106 | exit 1; |
| 107 | fi |
| 108 | |
| 109 | #Check if sled is present |
| 110 | SLED_PRESENT=$(get_gpio "presence-sled${SLED_NUM}") |
| 111 | if [ "$SLED_PRESENT" != 0 ];then |
| 112 | echo "${SLED} is not present, skip motor initialize" |
| 113 | exit 1 |
| 114 | fi |
| 115 | |
| 116 | #Init gpios |
| 117 | init_gpios "$SLED_NUM" |
| 118 | |
| 119 | #enable pwm |
| 120 | open_pwm "$SLED_NUM" |
| 121 | |
| 122 | #SLED{N}_MS_DETECT1 (initial position) |
| 123 | DETECT_PIN1="SLED${SLED_NUM}_MS_DETECT1" |
| 124 | INIT_POS=$(get_gpio "$DETECT_PIN1") |
| 125 | |
| 126 | if [ "$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 |
| 140 | fi |
| 141 | |
| 142 | if [ "$INIT_POS" -eq 0 ];then |
| 143 | echo "Motor calibrated to initial position." |
| 144 | exit 0 |
| 145 | else |
| 146 | echo "Find motor initial position failed" |
| 147 | exit 1 |
| 148 | fi |