gui: new tractbrake machinery, seems to mostly work for gamepad

also somewhat change the way inhibition works

still undone: select train from gamepad; uninhibit from gamepad
inhibition due to signalling problem is a bit ad-hoc

diff --git a/hostside/gui b/hostside/gui
index 854804ed0b63c94dacc6bd6b2ff455d63c9cfa0c..14e451c616ea74f2a8385a4eb72e373e983eb675 100755 (executable)
--- a/hostside/gui
+++ b/hostside/gui
@@ -392,6 +392,7 @@ proc speedw-notrains {w whystr} {
 }
 proc speedw-train-noneselected {w whystr} {
     upvar #0 speed/$w s
+    tractbrake-detach $s(train)
     set s(train) {}
     $w.train configure -text $whystr
     $w.speed configure -text -
@@ -430,6 +431,8 @@ proc speedw-train-selectnext {w} {
 
 proc speedw-train-selected {w t} {
     upvar #0 speed/$w s
+    if {![string compare $t $s(train)]} return
+    tractbrake-detach $s(train)
     $w.train configure -text $t
     set s(inhibit) 1
     set s(train) $t
@@ -483,6 +486,20 @@ proc speedw-trains-available {w l} {
 proc speedw-userinput-abs {w speed} {
     upvar #0 speed/$w s
     if {![string length $s(train)]} return
+    tractbrake-detach $s(train)
+    if {!$speed} { speedw-uninhibit $w 2 }
+    speedw-do-abs $w $speed
+}
+
+proc speedw-userinput-tractbrake {w tract brake} {
+    upvar #0 speed/$w s
+    if {![string length $s(train)]} return
+    if {$s(inhibit)} return
+    tractbrake-userinput $s(train) $tract $brake $w
+}
+
+proc speedw-do-abs {w speed} {
+    upvar #0 speed/$w s
     set s(queued) $speed
     speedw-check $w
 }
@@ -496,10 +513,7 @@ proc speedw-check {w} {
     if {![info exists s(queued)]} return
     set newspeed $s(queued)
     unset s(queued)
-    if {$s(inhibit)} {
-       if {$newspeed > $gcommanded} return
-       speedw-uninhibit $w 2
-    }
+    if {$s(inhibit) && $newspeed > $gcommanded} return
     set s(commanding) $newspeed
     scmd speedw-commanded $s(ctrlr) "speed $s(train) $newspeed $gdirection" $w
 }
@@ -517,17 +531,22 @@ proc speedw-commanded-err {m w} {
     speedw-check $w
 }
 
-proc speedw-userinput-rel {w stepmap} {
+proc speedw-currentspeed {w} {
     upvar #0 speed/$w s
-    if {![string length $s(train)]} return
     upvar #0 train_commanded($s(train)) gcommanded
     if {[info exists s(queued)]} {
-       set oldspeed $s(queued)
+       return $s(queued)
     } elseif {[info exists s(commanding)]} {
-       set oldspeed $s(commanding)
+       return $s(commanding)
     } else {
-       set oldspeed $gcommanded
+       return $gcommanded
     }
+}
+
+proc speedw-userinput-rel {w stepmap} {
+    upvar #0 speed/$w s
+    if {![string length $s(train)]} return
+    set oldspeed [speedw-currentspeed $w]
     set newspeed [eval $stepmap [list $oldspeed]]
     speedw-userinput-abs $w $newspeed
 }
@@ -542,6 +561,13 @@ proc speedw-userinput-rel-steps {w delta steplist} {
     speedw-userinput-rel $w [list speedw-stepmap-fromlist $steplist $delta]
 }
 
+proc speedw-userinput-tractbrake {w tract brake} {
+    upvar #0 speed/$w s
+    if {![string length $s(train)]} return
+    speedw-uninhibit $w 1
+    tractbrake-userinput $s(train) $tract $brake $w
+}
+
 proc speedws-train-problem {train} {
     speedws-fortrain $train speedw-inhibit
 }
@@ -551,6 +577,7 @@ register-event ?train_*_at {train direction} \
     upvar #0 train_direction($train) dirn
     set dirn $direction
     speedws-fortrain $train speedw-redisplay-speed
+    tractbrake-ensure $train
 }
 
 register-event ?train_*_speed_commanding {train speed} \
@@ -613,6 +640,162 @@ proc speedw-stepmap-fromlist {speedlist offset oldspeed} {
     return [lindex $speedlist $ix]
 }
 
+#----- traction / brake (hidden behind speedw) ----------
+
+proc tractbrake-queue-update {train} {
+    upvar #0 tractbrake/$train tb
+    set tb(queued) [after $tb(updms) \
+                       [list tractbrake-update $train]]
+}
+
+proc tractbrake-attach {train speedw} {
+    # can safely be called when already attached
+    upvar #0 tractbrake/$train tb
+    upvar #0 speedcurve/$train sc
+    set tb(speedw) $speedw
+    if {[info exists tb(queued)]} return
+    upvar #0 train_commanded($train) gcommanded
+    setexpr tb(v) {[lindex $sc $gcommanded] / [lindex $sc 126]}
+    tractbrake-queue-update $train
+}
+proc tractbrake-detach {train} {
+    # can safely be called when already detached
+    upvar #0 tractbrake/$train tb
+    catch { after cancel $tb(queued) }
+    catch { unset tb(queued) }
+    tractbrake-reset $train
+}
+proc tractbrake-reset {train} {
+    upvar #0 tractbrake/$train tb
+    if {![info exists tb]} return
+    set tb(A) 0
+    set tb(B) 0
+    set tb(a) 0
+    set tb(b) 0
+}
+
+proc tractbrake-update {train} {
+    upvar #0 tractbrake/$train tb
+    upvar #0 tractbrake-params/$train pa
+    unset tb(queued)
+    foreach AB {A B} ab {a b} lm {lambda mu} {
+       addexpr tb($ab) { $tb(updfact_$lm) * ( $tb($AB) - $tb($ab) ) - 1e-5 }
+    }
+    addexpr tb(v) {
+       + $tb(perupd_alpha) * $tb(a)
+       - $tb(perupd_beta) * $tb(b)
+       - $tb(perupd_omega) * $tb(v) * $tb(v)
+       - $tb(perupd_phi)
+    }
+    set m "tractbrake $train"
+    foreach v {A a B b v} { append m [format " %s=%6.4f" $v $tb($v)] }
+    debug $m
+    if {$tb(v) <= 0} {
+       # stopped
+       set tb(v) 0
+       speedw-do-abs $tb(speedw) 0
+       if {$tb(A) <= 0 && $tb(a) <= 0 &&
+           $tb(B) <= 0 && $tb(b) <= 0} {
+           # no throttle or brake, no need to requeue
+           return
+       }
+    } else {
+       if {$tb(v) > 1.0} { set tb(v) 1.0 }
+       upvar #0 speedcurve/$train sc
+       upvar #0 train_commanded($train) gcommanded
+       set step $gcommanded
+       setexpr targetvel {$tb(v) * [lindex $sc 126]}
+       while 1 {
+           set vel [lindex $sc $step]
+           if {$vel > $targetvel && $step > 0} {
+               setexpr nextstep {$step - 1}
+           } elseif {$vel < $targetvel && $step < 126} {
+               setexpr nextstep {$step + 1}
+           } else {
+               break
+           }
+           set nextvel [lindex $sc $nextstep]
+           if {abs($nextvel-$targetvel) >= abs($vel-$targetvel)} {
+               break
+           }
+           set step $nextstep
+       }
+       if {$step != $gcommanded} {
+           speedw-do-abs $tb(speedw) $step
+       }
+    }
+    tractbrake-queue-update $train
+}
+
+proc tractbrake-userinput {train tract brake speedw} {
+    upvar #0 tractbrake/$train tb
+    if {![info exists tb]} {
+       report-problem "event: no traction/brake parameters for $train"
+       return
+    }
+    if {[string length $tract]} { set tb(A) $tract }
+    if {[string length $brake]} { set tb(B) $brake }
+    if {$tract || $brake} {
+       tractbrake-attach $train $speedw
+    }
+}
+
+proc tractbrake-ensure {train} {
+    upvar #0 speedcurve/$train sc
+    if {[info exists sc]} return ;# try this only once
+    set sc 0
+
+    if {[regexp {[^-+._0-9a-z]} $train]} { error "bad train $train ?" }
+
+    if {[catch { set f [open $train.speeds.record] } emsg]} {
+       global errorCode errorInfo
+       switch -glob $errorCode {POSIX ENOENT *} {
+           report-problem "train $train: no traction/braking (no speed table)"
+           return
+       }
+       error $emsg $errorInfo $errorCode
+    }
+    while {[llength $sc] <= 126} { lappend sc x }
+    while {[gets $f l] >= 0} {
+       if {![regexp {^train (\S+) step (\d+)=([0-9.]+)$} $l \
+                 dummy tr step velocity]} continue
+       if {[string compare $tr $train] || $step<=0 || $step>126} {
+           error "bad velocity line $l ?" {} {TRACTBRAKE SKIP}
+       }
+       set sc [lreplace $sc $step $step $velocity]
+    }
+    close $f
+    if {[lsearch -exact $sc x]>=0} {
+       report-problem "train $train: no traction/braking\
+            (incomplete speed table"
+       return
+    }
+
+    upvar #0 tractbrake/$train tb
+    defset tb(deadzone) 0.2
+    defset tb(updms)    20
+    defset tb(lambda)   0.600  ;# time constant for adj throttle
+    defset tb(mu)       0.300  ;# time constant for apply/release breaks
+    defset tb(inv_alpha)   20  ;# time constant for accelerate to max
+    defset tb(omegaphi)    50  ;# (air resistance) / (rolling res) at max spd
+    defset tb(inv_beta)    10  ;# time constant for service brake (over-est'd)
+    defset tb(overpower) 1.03  ;# factor by which we are overpowered for max spd
+    foreach lm {lambda mu} {
+       setexpr tb(updfact_$lm) { $tb(updms) * 0.001 / $tb($lm) }
+    }
+
+    setexpr tb(alpha) { 1.0 / $tb(inv_alpha) }
+    setexpr tb(beta) { 1.0 / $tb(inv_beta) }
+    setexpr tb(phi) { $tb(alpha) / ($tb(omegaphi) + 1.0) / $tb(overpower) }
+    setexpr tb(omega) { $tb(omegaphi) * $tb(phi) }
+
+    foreach p {alpha beta omega phi} {
+       setexpr tb(perupd_$p) { $tb($p) * 0.001 * $tb(updms) }
+    }
+
+    tractbrake-reset $train
+}
+
 #---------- concrete input bindings ----------
 
 proc ib-suppressions {args} {
@@ -672,6 +855,25 @@ proc ib-evcmd/wheelmouse {devid target} {
 
 proc ib-create/gamepad {devid wunique desc} {
     ib-speedw-new $devid $wunique $desc
+    upvar #0 input/$devid in
+    set in(tractbrake_deadzone) 0.2
+}
+
+proc ib-ev/tractbrake/init {devid} {
+    upvar #0 input-params/$devid pa
+}
+
+proc ib-ev/gamepad/EV_ABS/ABS_THROTTLE {devid value} {
+    upvar #0 input/$devid in
+    if {abs($value) < $in(tractbrake_deadzone)} {
+       speedw-userinput-tractbrake $in(speedw) 0 0
+       return
+    }
+    if {$value > 0} {
+       speedw-userinput-tractbrake $in(speedw) $value 0
+    } {
+       speedw-userinput-tractbrake $in(speedw) 0 [expr {-$value}]
+    }
 }
 
 #----- ebuyer wireless keyboard