#------------------------------------------------------------------
# This is driver for the combination Phytron MCC-2 Motor Controller 
# and SICS using the scriptcontext asynchronous I/O system. The 
# MCC-2 has a funny protocl as that messages are enclosed into 
# <STX>  data <ETX> sequences. This protocol is handled by the 
# C-language phytron protocol handler. Per default, the MCC-2 is 
# configured to use 57600 baud. I have configured it to use 9600
# baud and it ought to remember this. The command to change this 
# 0IC1S9600, the command to read this is 0IC1R. 
#
# So, if this thing does not work on a serial port then the solution is 
# to set the terminal server to 57600 and try again. And set the baud rate
# or leave it.
#
# There are surely many ways to use the MCC-2. It supports two axes, X and Y. 
# All examples below are given for X only. This driver uses it in
# this way: 
# 
# Nothing works properly without a reference run. The reference run is done 
# in  the following way:
# 1) Send it into the negative limit switch with 0X0-   
# 2) Set the mechanical position with 0XP20Swert to the negative limit
# 3) Set the encoder position with 0XP22Swert to the negative limit
#
# Position ever afterwards with 0XAwert, read encoder with 0XP22R
#
# While driving 0X=H return ACKN, else ACKE
#
# Stopping goes via 0XSN
#
# copyright: see file COPYRIGHT
#
# Script chains:
#
# - reading position:
#    readpos - posrcv
#
# - writing postion:
#    setpos - setrcv
#
# - reading status:
#     sendstatus - rcvstatus - statpos
#
# - reading speed:
#     readspeed - rcvspeed
#
# - setting speed:
#     writespeed - rcvwspeed - rcvspeed
# 
# Mark Koennecke, June 2009
#
# Added code to switch a brake on for schneider_m2
#
# Mark Koennecke, September 2009
#
# Added code to support the speed parameter
# 
# Mark Koennecke, December 2009
# TODO: speed still has to be tested: 02-12-2009
#-------------------------------------------------------------------------

namespace eval phytron {}

#-----------------------------------------------------------------------
proc phytron::check {} {
    set data [sct result]
    if {[string first AscErr $data] >= 0} {
	error $data
    }
    return $data
}
#------------------------------------------------------------------------
proc phytron::readpos {axis} {
    sct send "0${axis}P22R"
    return posrcv
}
#------------------------------------------------------------------------
proc phytron::posrcv {} {
    set data [phytron::check]
    set pos [string range $data 3 end]
    sct update $pos
    return idle
}
#------------------------------------------------------------------------
proc phytron::setpos {axis name} {
    set val [sct target]
    sct send "0${axis}A$val"
    hupdate /sics/${name}/status run
    return setrcv
}
#------------------------------------------------------------------------
proc phytron::setrcv {controller name} {
    set data [phytron::check]
    if {[string first NACK $data] >= 0} {
	error "Invalid command"
    }
    $controller queue /sics/${name}/status progress read
    return idle
}
#-------------------------------------------------------------------------
proc phytron::sendstatus {axis} {
    sct send "0${axis}=H"
    return rcvstatus
}
#-------------------------------------------------------------------------
proc phytron::rcvstatus {axis controller} {
    set status [catch {phytron::check} data]
    if {$status != 0} {
	sct update error
	clientput $error
    }
    if {[string first ACKN $data] >= 0} {
	sct update run
	$controller queue [sct] progress read
    }
    if {[string first ACKE $data] >= 0} {
	phytron::readpos $axis
	return posrcv
    }
    return idle
}
#-------------------------------------------------------------------------
proc phytron::statpos {axis name} {
    set data [phytron::check]
    set pos [string range $data 3 end]
    hupdate /sics/${name}/hardposition $pos
    sct send "0${axis}=I+"
    return statposlim
}
#------------------------------------------------------------------------
proc phytron::statposlim {axis} {
    set data [phytron::check]
    if {[string first ACKE $data] >= 0} {
	sct update error
	clientput "Hit positive limit switch"
        return idle
    }
    sct send "0${axis}=I-"
    return statneglim
}
#------------------------------------------------------------------------
proc phytron::statneglim {axis} {
    set data [phytron::check]
    if {[string first ACKE $data] >= 0} {
	sct update error
	clientput "Hit negative limit switch"
        return idle
    }
    sct send "0${axis}=E"
    return statend
}
#------------------------------------------------------------------------
proc phytron::statend {axis} {
    set data [phytron::check]
    if {[string first ACKE $data] >= 0} {
	sct update error
	clientput "Electronics error"
        return idle
    }
    sct update idle
    return idle
}
#------------------------------------------------------------------------
proc phytron::readspeed {axis} {
    sct send "0${axis}P14R"
    return rcvspeed
}
#------------------------------------------------------------------------
proc phytron::rcvspeed {} {
    set data [phytron::check]
    set speed [string range $data 3 end]
    sct update $speed
    return idle
}
#------------------------------------------------------------------------
proc phytron::writespeed {axis} {
    set val [sct target]
    sct send "0${axis}P14S$val"
    return rcvwspeed
}
#------------------------------------------------------------------------
proc phytron::rcvwspeed {axis} {
    set data [phytron::check]
    if {[string first NACK $data] >= 0} {
	error "Invalid command"
    }
    return [phytron::readspeed $axis]
}
#-------------------------------------------------------------------------
proc phytron::halt {controller axis} {
    $controller send "0${axis}SN"
    return Done
}
#--------------------------------------------------------------------------
proc phytron::refrun {name controller axis lowlim} {
    set path /sics/${name}/status
    $controller send "0${axis}0-"
    hupdate $path run
    set motstat run
    wait 3
    while {[string compare $motstat run] == 0} {
	$controller queue $path progress read
	wait 1
	set motstat [string trim [hval $path]]
    }
    $controller transact "0${axis}P20S$lowlim"
    $controller transact "0${axis}P22S$lowlim"
    return Done
}
#-------------------------------------------------------------------------
proc phytron::defpos {controller axis value} {
    $controller transact "0${axis}P20S$value"
    $controller transact "0${axis}P22S$value"
    return Done
}
#--------------------------------------------------------------------------
proc phytron::make {name axis controller lowlim upperlim} {
    MakeSecMotor $name

    hdel /sics/${name}/hardupperlim
    hdel /sics/${name}/hardlowerlim
    hfactory /sics/${name}/hardupperlim plain internal float
    hfactory /sics/${name}/hardlowerlim plain internal float
    $name hardlowerlim $lowlim
    $name softlowerlim $lowlim
    $name hardupperlim $upperlim
    $name softupperlim $upperlim
 
    hsetprop /sics/${name}/hardposition read phytron::readpos $axis
    hsetprop /sics/${name}/hardposition posrcv phytron::posrcv 
    $controller poll /sics/${name}/hardposition 60

    hsetprop /sics/${name}/hardposition write phytron::setpos $axis $name
    hsetprop /sics/${name}/hardposition setrcv phytron::setrcv $controller $name
    $controller write /sics/${name}/hardposition
    
    hsetprop /sics/${name}/status read phytron::sendstatus $axis
    hsetprop /sics/${name}/status rcvstatus phytron::rcvstatus $axis $controller
    hsetprop /sics/${name}/status posrcv  phytron::statpos $axis $name
    hsetprop /sics/${name}/status statposlim  phytron::statposlim $axis 
    hsetprop /sics/${name}/status statneglim  phytron::statneglim $axis 
    hsetprop /sics/${name}/status statend  phytron::statend $axis 
    $controller poll /sics/${name}/status 60

    hfactory /sics/${name}/speed plain user float
    hsetprop /sics/${name}/speed read "phytron::readspeed $axis"
    hsetprop /sics/${name}/speed rcvspeed "phytron::rcvspeed"
    hsetprop /sics/${name}/speed write "phytron::writespeed $axis"
    hsetprop /sics/${name}/speed rcvwspeed "phytron::rcvwspeed $axis"
    $controller poll /sics/${name}/speed 60
    $controller write /sics/${name}/speed

    $name makescriptfunc halt "phytron::halt $controller $axis" user

    $name makescriptfunc refrun "phytron::refrun $name $controller $axis $lowlim" user

    $name makescriptfunc sethardpos "phytron::defpos $controller $axis" user
    hfactory /sics/${name}/sethardpos/value plain user float

    hupdate /sics/${name}/status idle
    $controller queue /sics/${name}/hardposition progress read
    $controller queue /sics/${name}/speed  progress read
} 
#===============================================================================================
# At MORPHEUS there is a special table where one motor needs a brake. This requires a digital I/O 
# to be disabled before driving and enabled after driving. The code below adds this feature to
# a phytron motor
#-----------------------------------------------------------------------------------------------
proc phytron::openset {out} {
    sct send [format "0A%dS" $out]
    return  openans
}
#----------------------------------------------------------------------------------------------
proc phytron::openans {axis name} {
    after 100
    return [phytron::setpos $axis $name]
}
#----------------------------------------------------------------------------------------------
proc phytron::outsend {axis out} {
    set data [phytron::check]
    if {[string first ACKE $data] >= 0} {
	sct update error
	clientput "Electronics error"
        return idle
    }
    sct send [format "0A%dR" $out]
    return outend
}
#----------------------------------------------------------------------------------------------
proc phytron::outend {} {
    sct update idle
    return idle
}
#----------------------------------------------------------------------------------------------
proc phytron::configureM2 {motor axis out} {
    set path /sics/${motor}
    hsetprop $path/hardposition write phytron::openset $out
    hsetprop $path/hardposition openans phytron::openans $axis $motor

    hsetprop $path/status statend  phytron::outsend $axis $out 
    hsetprop $path/status outend   phytron::outend
}