#!/bin/bash
# (C) jOERG Reisenweber 2007-08-05
#
# Der Referenzschalter muss pro Umderhung des Tellers einmal am Nullpunkt kurz *oeffnen*.
# Anschluss an blau und schwarz vom Modul.
#
# Installation:
# companastepII und camcontrol im searchpath ablegen und executable setzen.
# link in /dev von "ttyTMC" auf die tatsaechliche RS232-Schnittstelle erzeugen, Rechte anpassen
#
# mindestens einmalig zum Kalibrieren der Anzahl Steps/Umdrehung, sowie optional Speichern
# von max-speed und max-acceleration:
# > companastepII -I -s 300 -a 60 "/home/photo/download";
# interaktiv aus der shell aufrufen. Gibt einige Klartextmeldungen
#
# Normale Benutzung:
# > companastepII -i "--zoom=5 --flash=off"; zum "Einschalten"
# > companastepII -v -n 12 -s 150 -a 10; 12 Fotos schiessen, fuer diesen Lauf eigene s & a Werte verwenden
# > companastepII -q; Kamera abschalten
#
# -c fuer Sonder und Debug Zwecke, z.B "Drehteller"
# > companastepII -c "ROR 0,200"; Rechtsdrehen mit 200, endlos. Siehe TMCL-Doku
# > companastepII -c "GGP 1,2"; Steps/Rotation aus Modul auslesen

USAGE="Usage: `basename $0` options camera-options\n
`basename $0` -I [-s] [-a]	#store speed, acceleration\n
`basename $0` -i | -q\n
`basename $0` -n [-s] [-a] [camopt]\n
\n
options:\n
-i	: init. Power-on reset, use prev. calibration\n
-I	: full init. Calibrate devices, pesistently store -s -a\n
-q	: quit\n
-s <n>	: speed\n
-a <n>	: acceleration\n
-n <n>	: number of stops/fotos\n
-v	: verbose\n
\n
-c <TMCLcmd>	: direct TMCL cmd\n
"

set -e -u


DEBUG=false
VERBOSE=false
#VERBOSE=true; DEBUG=true; set -v -x

#Systemparameter
Tty=/dev/ttyTMC
#Tty=/dev/ttyS0		#Anschluss des PD110, wird mit `cd /dev; ln -s ttyS0 ttyTMC` im System parametriert
TtyBaud=9600		#9600 ist sowohl PD110-factory-preset als auch Unix-Standard.
			# MUSS mit der Einstellung im Modul uebereinstimmen
Sys_Delay=0.1		#Vorlaufzeit zum Aufsetzen des "Empfaengers" vor senden des Cmds. 
			# Abhaengig v. Proz-takt, Datenrate(TtyBaud), Puffergroesse RS232-HW, nice, anderen Tasks...
			# Bei verstuemmelter Datenuebertragung vom Modul hoeher setzen.
FullRotation=12800	#Steps/Tellerdrehung. 12800 = eine 360° Umdrehung der PD110-Motorachse
			# Bei einer Untersetzung von N/1 muss dieser Wert also 12800*N sein
			# Die urspruenglich geplante automatische Kalibrierung konnte ohne
			# entsprechende Hardware nicht entwickelt und getestet werden
SettleTime=1.2		#Zeit in Sek , nach Tellerstop bis Ausloesen Kamera - 
			# zum Beruhigen elastischer oder fluessiger Objekte

#constants
TakeFoto=camcontrol	#comand (script) welches die Kameraansteuerung uebernimmt. $1=TF_xxx, $2(o. 3) camopts
TF_on=s			#Kamera einschalten, Viewfinder starten
TF_whiteb=w		#TF_on + Weissabgleich
TF_start=s		#Ankündigung einer Aufnahmesequenz. Blitz scharfmachen etc
TF_capture=c		#Ausloesen. Aufruf mit $2 = fortlaufende Fotonummer fuer diesen Durchgang, $3 = camopts
TF_done=d		#Sequenz fertig. Blitz aus etc. Evtl download der Daten
TF_quit=q		#Ausschalten, Linse einfahren

ERROR_HW=2
ERROR_TMCL=3
ERROR_OPT=65

GP_spr=1		#Modul NVRAM Global_Par Bank2 Variable n. Dort wird FullRotation gespreichert

#
#Axis Parameters. TMCL_reference.pdf p.51
# 4.1 Basic axis parameters on the TMCM-3xx / 11x /
#     109 and 61x modules

#Axis = 0 for Pandrive110
# 0 target (next) position
AP_actual_position=1
# 2  target (next) speed
# 3  actual speed
AP_maximum_positioning_speed=4
AP_maximum_acceleration=5
# 6  absolute max. current
# 7  standby current
AP_target_pos_reached=8
AP_refswitch_status=9
# 10 right limit switch status
# 11 left limit switch status
# 12 right limit switch disable
# 13 left limit switch disable
AP_steprate_prescaler=14
#special
# 130 minimum speed
# 135 actual acceleration
# 136 acceleration threshold 
# 137 acceleration divisor
# 138 ramp mode
# 139 interrupt flags
# 140 microstep resolution
# 141 ref. switch tolerance
AP_snapshot_position=142
# 143 max. current at rest
# 144 max. current at low
#     accel.
# 145 max. current at high
#     accel.
# 146 acceleration factor
# 147 ref. switch disable flag
# 148 limit switch disable
#     flag
# 149 soft stop flag
# 150 (reserved)
AP_position_latch_flag=151
# 152 interrupt mask
# 153 ramp divisor
# 154 pulse divisor
AP_referencing_mode=193
# 194 referencing search
#     speed
# 195 referencing switch
#     speed
# 196 (reserved)
# 197 (reserved)
# 198 driver off time
# 200 fast decay time
# 203 mixed decay
#     threshold
# 204 freewheeling
# 205 Stall Detection
#     Threshold
# 206 Actual Load Value
# 208 Driver Error Flags
# 209 Encoder Position
#     (V3.21 or higher)
# 210 Encoder Pre-scaler
#     (V3.21 or higher)
# 211 Fullstep Threshold
#     (V3.26 or higher)

# #
# #Global Parameters
# #
# 64 EEPROM magic
# 65 RS232 and RS485
#    baud rate
#** preconfic ** muss mit stty Parameter uebereinstimmen

# 66 Serial address
#** preconfic ** das "A". Andere Adressen werden nicht unterstuetzt

# 67 ASCII mode
#    (available since
#    version 3.21)
#** preconfic ** 
# Configure the TMCL ASCII interface:
# Bit 0: 0 – start up in binary (normal) mode
#        1 – start up in ASCII mode <-set!
# Bits 4 and 5:
# 00 – Echo back each character
# 01 – Echo back complete command <-set!
#** preconfic ** 
# 10 – Do not send echo, only send command reply


# 68 Reserved
# 69 CAN bit rate
# 70 CAN reply ID
# 71 CAN ID
# 72 System error mask
# 73 Configuration
#    EEPROM lock flag
# 74 Encoder interface
#    (available since
#    version 3.21)
# 75 Telegram pause time
# 76 Serial host address
# 77 Auto start mode
# 78 Poll interval
# 79 Port function mask
# 80 Shutdown pin
#    functionality
# 81 TMCL code protection
# 128 TMCL application
#     status
# 129 Download mode
# 130 TMCL program counter
# 131 Application error flags 
# 132 Tick timer




Flush_ttySn()
{
	while read -t 1 -u 4 -n 1; do :; done
}



TMCL_cmd()
{
	wait="1"
	(sleep $Sys_Delay; echo -ne "A $@\r">$Tty)&
	
	a=;b=;c=;d=
	if read -u4 -t $wait a;
	then
		$DEBUG && echo "Echo: $a" >&2
	else
		rc=$?
		echo "read Cmd-Echo failed: rc:$rc. 1=Timeout. Check Module and RS232-cable" >&2
		exit $ERROR_HW
	fi

	if [ "A $@"x != "$a"x ]
	then
		echo "read Cmd-Echo failed: Noise. Check Module and RS232-cable" >&2
		echo "Cmd:  $@" >&2
		echo "Echo: $a" >&2
		exit $ERROR_HW
	fi
		
	$DEBUG && echo "read reply" >&2
	a=;b=;c=;d=
	if read -u4 -t $wait a b c d;
	then
		$DEBUG && echo "Answer: $a | $b | $c | $d" >&2
	else
		rc=$?
		echo "read Answer from Module failed: rc:$rc. 1=Timeout. Check Module and RS232-cable" >&2
		exit $ERROR_HW
	fi

	if [ "$b" -ne 100 ] 
	then
		echo -e "Module reports ERROR:\nCmd: $@\nModule-Answer: $a | $b | $c | $d" >&2
		exit $ERROR_TMCL
	fi

	$DEBUG && echo "=======" >&2
	echo $c
}



TMCL_get_axis_parameter()
{
	local AxisParameter=$1
	echo `TMCL_cmd "GAP $AxisParameter,0"`
}



TMCL_set_axis_parameter()
{
	local AxisParameter=$1
	local Value=$2
	TMCL_cmd "SAP $AxisParameter,0, $Value" >/dev/null
}



TMCL_store_axis_parameter()
{
	local AxisParameter=$1
	local Value=$2
	TMCL_set_axis_parameter $AxisParameter $Value &&
	TMCL_cmd "STAP $AxisParameter,0" >/dev/null
}



TMCL_restore_axis_parameter()
{
	local AxisParameter=$1
	TMCL_cmd "RSAP $AxisParameter,0" >/dev/null
}



TMCL_wait_finish()
{
	$VERBOSE && echo -en "\nmoving " >&2
	while [ "`TMCL_get_axis_parameter ${AP_target_pos_reached}`" != "1" ]
	do
		sleep 0.1
		$VERBOSE && echo -n "->" >&2
	done
	$VERBOSE && echo -e "X\n" >&2
	return 0
}

TMCL_refsearch()
{
	TMCL_cmd "RFS START, 0" >/dev/null
	$VERBOSE && echo -en "\ncalibrating " >&2
	while [ "`TMCL_cmd "RFS STATUS,0"`" -ne "0" ]
	do
		sleep 0.1
		$VERBOSE && echo -n "->" >&2
	done
	$VERBOSE && echo -e " OK\n" >&2
	return 0
}


Shot_sequence()
{
	NUM_SEGMENTS=$1
	STEPS_PER_SEG=$(( FullRotation / NUM_SEGMENTS ))
	eval ${TakeFoto} $TF_start "$camopts"
	$DEBUG && echo "Segmente:$NUM_SEGMENTS   Steps/Segment:$STEPS_PER_SEG"
	for ((i=1; i<=$NUM_SEGMENTS; i++))
	do
		$VERBOSE && echo "Pos: $i of $NUM_SEGMENTS" >&2
		TMCL_cmd "MVP REL, 0, $STEPS_PER_SEG" >/dev/null
		TMCL_wait_finish
		sleep "${SettleTime}"
		eval ${TakeFoto} $TF_capture $i "$camopts"
	done
	TMCL_cmd "MVP ABS, 0, 0" >/dev/null
	eval ${TakeFoto} $TF_done "$camopts"
	TMCL_wait_finish
}



Quit()
{
	TMCL_cmd "MVP ABS, 0, 0" >/dev/null
	eval ${TakeFoto} $TF_quit "$camopts"
	exit
}





# main
OPTIONS="s:a:n:vIiqc:"
if [ $# -eq 0 ] # Script invoked with no command-line args?
then
	echo -e $USAGE
	exit $ERROR_OPT
fi

speed=;accel=;steps=;init=n
while getopts "$OPTIONS" Option
do
	case $Option in
		s ) speed="$OPTARG";;
		a ) accel="$OPTARG";;
		n ) steps="$OPTARG";;
		v ) VERBOSE=true;;
		i ) init="i";;
		I ) init="I";;
		q | c ) break;;
		* ) echo "unknown option: $OPTARG"
		    echo $USAGE
		    exit $ERROR_OPT;;
	esac
done
shift $(($OPTIND - 1))
camopts=$@
#ToDo: checking for valid set of options and option value types and limits, for luxury version

exec 4<$Tty
$DEBUG && echo "$@" >&2
stty $TtyBaud raw -echo icrnl<&4

#stop motor when killed
trap "TMCL_cmd 'MST 0'>/dev/null;Flush_ttySn;TMCL_cmd 'MST 0'>/dev/null;echo 'aborted, Motor stopped'>&2;exit" 1 2 3 15 ERR

Flush_ttySn

tmp=`TMCL_cmd "GGP $GP_spr, 2"` 
$DEBUG && echo "Steps/rotation read from module: $tmp" >&2
if [ $tmp -gt 0 ]
then
	FullRotation=$tmp
	$DEBUG && echo "FullRotation restored: $FullRotation" >&2
else
	$DEBUG && echo "FullRotation default: $FullRotation" >&2
fi


if [ $Option == q ]
then
	Quit
	exit
fi


if [ $Option == c ]
then
	DEBUG=true
	TMCL_cmd "$OPTARG"
	exit
fi


if [ -n "$steps" ]
then
	if [ -n "$speed" ]
	then
		TMCL_set_axis_parameter $AP_maximum_positioning_speed $speed
	else
		TMCL_restore_axis_parameter $AP_maximum_positioning_speed 
	fi

	if [ -n "$accel" ]
	then
		TMCL_set_axis_parameter $AP_maximum_acceleration $accel
	else
		TMCL_restore_axis_parameter $AP_maximum_acceleration
	fi

	Shot_sequence $steps
	exit
fi


if [ $init == I ]
then
	if [ -n "$speed" ]
	then
		echo "Persistantly store default max. speed: $speed">&2
		TMCL_store_axis_parameter $AP_maximum_positioning_speed $speed
	fi

	if [ -n "$accel" ]
	then
		echo "Persistantly store default max. acceleration: $accel">&2
		TMCL_store_axis_parameter $AP_maximum_acceleration $accel
	fi
#ToDo: One full turn to refswitch, read position, store persistantly for $FullRotation in Module's NVRAM
#edit: done. Bonus
	#rotate right with nomonal speed
	TMCL_refsearch

	echo "Starting calibration of steps/rotation...">&2
#	TMCL_set_axis_parameter $AP_actual_position 0
	TMCL_cmd "MVP ABS, 0, 8000000" >/dev/null
	#eventually move out if refswitch range. Switches are NeutralClosed to 0V, i.e. active=1
	while [ "`TMCL_get_axis_parameter ${AP_refswitch_status}`" = "1" ]
	do
		sleep 0.1
	done
	echo "Waiting for first refswitch pulse...">&2
	TMCL_set_axis_parameter ${AP_snapshot_position} 123
	TMCL_set_axis_parameter ${AP_position_latch_flag} 123
#position_latch_flag doen't work as supposed. Though snapshot_position catches pos, flag never is set.
#	while [ "`TMCL_get_axis_parameter ${AP_position_latch_flag}`" = "0" ]
#	do
#		sleep 0.1
#	done
#So we do a diry workaround by polling and comparing values of snapshot_position to detect capture
# This is NOT safe! It occationaly may fail if captured value is the same as prev. one. ->Stall.
	tmp="`TMCL_get_axis_parameter ${AP_snapshot_position}`"
	while [ "`TMCL_get_axis_parameter ${AP_snapshot_position}`" = $tmp ]
	do
		sleep 0.1
	done
	firsttime="`TMCL_get_axis_parameter ${AP_snapshot_position}`"
	echo "Got first position: $firsttime (relative)">&2



	while [ "`TMCL_get_axis_parameter ${AP_refswitch_status}`" = "1" ]
	do
		sleep 0.1
	done
	echo "Waiting for second refswitch pulse...">&2
	TMCL_set_axis_parameter ${AP_snapshot_position} 123
#	while [ "`TMCL_get_axis_parameter ${AP_position_latch_flag}`" = "0" ]
#	do
#		sleep 0.1
#	done
	tmp="`TMCL_get_axis_parameter ${AP_snapshot_position}`"
	while [ "`TMCL_get_axis_parameter ${AP_snapshot_position}`" = $tmp ]
	do
		sleep 0.1
	done
	secondtime="`TMCL_get_axis_parameter ${AP_snapshot_position}`"
	echo "Got second position: $secondtime (relative)">&2
	echo "Steps/rotation = $(( secondtime -  firsttime )). Storing persistently">&2
	TMCL_cmd "SGP $GP_spr, 2, $(( secondtime -  firsttime ))" >/dev/null
	TMCL_cmd "STGP $GP_spr, 2" >/dev/null

	eval ${TakeFoto} $TF_whiteb "$camopts"
	exit
fi


if [ $init == i ]
then
	if [ -n "$speed" ]
	then
		echo "-s ignored!">&2
	fi

	if [ -n "$accel" ]
	then
		echo "-a ignored!">&2
	fi
	#TMCL_cmd "MVP ABS, 0, 0" >/dev/null
	TMCL_refsearch
	eval ${TakeFoto} $TF_on "$camopts"
	exit
fi