#!/bin/sh

# Custom - Only intended for Kirk Wallace's HNC lathe, KW  20070823
# M101 - Reads current T word selection and rotates turret to
#        place matching tool slot in operating position. Like M6

# Read T
requested_tool=$(halcmd -s show pin iocontrol.0.tool-prep-number|cut -c 21)

# Read 4 bits from turret encoder and decode current turret position
ones=$(halcmd -s show pin ppmc.0.din.06.in|head -n 1|cut -c 20)
if [ $ones = "T" ]
then
  ones=1
else
  ones=0
fi
twos=$(halcmd -s show pin ppmc.0.din.07.in|head -n 1|cut -c 20)
if [ $twos = "T" ]
then
  twos=1
else
  twos=0
fi
fours=$(halcmd -s show pin ppmc.0.din.08.in|head -n 1|cut -c 20)
if [ $fours = "T" ]
then
  fours=1
else
  fours=0
fi
eights=$(halcmd -s show pin ppmc.0.din.09.in|head -n 1|cut -c 20)
if [ $eights = "T" ]
then
  eights=1
else
  eights=0
fi
current_tool=$(($ones+(2*$twos)+(4*$fours)+(8*$eights)))

# If requested tool is already in position - do nothing
if [ $requested_tool = $current_tool ]
then
  exit 0
fi

# Loop one position at a time until we get a match
while [ "$current_tool" != "$requested_tool" ]; do

# rotate and park one position
  halcmd sets TurretRotate True
  sleep .2
  halcmd sets TurretStop True
  sleep .2
  halcmd sets TurretRotate False
  sleep .1
  halcmd sets TurretStop False
  sleep .1

# Read and decode current position
  ones=$(halcmd -s show pin ppmc.0.din.06.in|head -n 1|cut -c 20)
  if [ $ones = "T" ]
  then
    ones=1
  else
    ones=0
  fi
  twos=$(halcmd -s show pin ppmc.0.din.07.in|head -n 1|cut -c 20)
  if [ $twos = "T" ]
  then
    twos=1
  else
    twos=0
  fi
  fours=$(halcmd -s show pin ppmc.0.din.08.in|head -n 1|cut -c 20)
  if [ $fours = "T" ]
  then
    fours=1
  else
    fours=0
  fi
  eights=$(halcmd -s show pin ppmc.0.din.09.in|head -n 1|cut -c 20)
  if [ $eights = "T" ]
  then
    eights=1
  else
    eights=0
  fi
  current_tool=$(($ones+(2*$twos)+(4*$fours)+(8*$eights)))
  echo $current_tool
done

## Check "turret locked" sensor and e-stop if not locked, otherwise
## exit
# turret_locked=$(halcmd -s show pin ppmc.0.din.10.in|head -n 1|cut -c 20)
# if [ $turret_locked != T ]
# then 
#   # insert link to e-stop
# else
# done
exit 0