#!/bin/sh
# @(#) $Header: /home/abrown/public_html/first/first2003chs/software/simulator/RCS/zipties.tcl,v 1.51 2006/12/14 19:40:16 abrown Exp $
# \
exec wish "$0" -geometry "480x600" "$@"

source {simfuncs.tcl}
source {tractorobject.tcl}
source {joystickobject.tcl}
source {servoobject.tcl}
source {linearobject.tcl}
source {ledobject.tcl}
source {outputledsobject.tcl}
source {simcontrol.tcl}

# -----------------------------------------------------------------
# zipties.tcl is a TCL program designed to exercise the CHS bsx
# code.  It is in no way essential to the pb2c translator.
# Rather it is an example of how to use the result of the
# translation.
#
# Copyright (C) 2002, 2003  Allen Brown
# 
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License version 2 as
# published by the Free Software Foundation.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the
#   Free Software Foundation, Inc.
#   59 Temple Place, Suite 330
#   Boston, MA  02111-1307  USA
#
# To contact the author of this software:
#   Allen Brown
#   PO Box J
#   Corvallis, OR
#
#   http://brown.armoredpenguin.com/~abrown/contact.html
# -----------------------------------------------------------------

# initial values
  set errors 0
  set cycle 0
  set p1_x 127
  set p1_y 127
  set p2_x 24; # flip control
  set p2_y 20; # was this 5?
  set sensor1 127; # gyro
  set sensor2 145
  set sensor3 120
  set oi_swA 0
  set oi_swB 0
  set rc_swA 0
  set rc_swB 0
  set outword 0
  set stackcont 0
  set heading 100

# ---- Function to update the graphics ------------
proc updatePos {} \
{
  global cycle logfileID pipeID outword
  global p1_x p1_y p2_x p2_y sensor2 sensor3
  global oi_swA oi_swB rc_swA rc_swB
  global joyItem Tractor SteeringState
  global simIntArray simIntArray_clk
  global joy_top joy_trig autoswitches autocont stackcont autoprog1 autoprog0
  global heading sensor1

  # oi_swA b0=joy_trig b1=joy_top
  # oi_swB b0=raise b1=lower
  # rc_swA stacker limits
  # rc_swB stacker limits, autonomous indicators
  # PB_mode
  # delta_t 0

  set p4_x 127
  set p3_x 127
  set PB_mode 127
  set packet_num 127
  set sensor4 127
  set p4_y 127
  set p3_y 127
  set sensor5 127
  set sensor6 127
  set p2_wheel 127
  set p1_wheel 127
  set sensor7 127
  set sensor8 127

  set p4_wheel 127
  set p3_wheel 127
  set p2_aux 127
  set p1_aux 127
  set p4_aux 127
  set p3_aux 127
  set delta_t 0
  set res01 127

  set oi_swA [SetBit [SetBit $oi_swA 1 $joy_top] 0 $joy_trig]
  set oi_swB [SetBit [SetBit $oi_swB 1 $stackcont/2] 0 $stackcont]
  set autoswitches [expr $autocont | $autoprog1 | $autoprog0]
  set rc_swB [expr ($rc_swB & 240) | $autoswitches]
  puts "----------------------------------------------"
  puts "autoswitches=$autoswitches rc_swB=$rc_swB"

  puts $pipeID "$oi_swA $oi_swB $rc_swA $rc_swB $p2_x $p1_x $PB_mode $sensor1 $sensor2 $p2_y $p1_y $sensor3 $p2_wheel $p1_wheel $delta_t"
  puts "serin: oi_swA=$oi_swA oi_swB=$oi_swB rc_swA=$rc_swA rc_swB=$rc_swB p2_x=$p2_x p1_x=$p1_x PB_mode=$PB_mode sensor1=$sensor1 sensor2=$sensor2 p2_y=$p2_y p1_y=$p1_y sensor3=$sensor3 p2_wheel=$p2_wheel p1_wheel=$p1_wheel delta_t=$delta_t"
#  puts "tcl: $p1_x $p1_y $p2_x"
  flush $pipeID
  set token "unmatched"
  while { $token != "pbserout:" }\
  {
    gets $pipeID pipeout
    puts $logfileID $pipeout
    set linearray [split $pipeout]
    set token [lindex $linearray 0]
    if { $token == "output:" }\
    {
      set outword [lindex $linearray 1]
    } else {
	if { $token == "ERROR" \
	     ||  $token == "DEBUG:" \
	     ||  $token == "debug:" }\
	{
	    puts $pipeout
	}
    }
  }
  puts "$pipeout"
  regexp {pwm1=([0-9]*) *relayA=([0-9]*) *pwm2=([0-9]*) *relayB=([0-9]*) *pwm3=([0-9]*) *pwm4=([0-9]*) *pwm5=([0-9]*) *pwm6=([0-9]*) *pwm7=([0-9]*)} \
	  $pipeout token pwm1 relayA pwm2 relayB pwm3 pwm4 pwm5 pwm6 pwm7
  #Tractor_set {Tractorarray} {tractL} {tractR} {xDirection} {yDirection}
  Tractor_set $Tractor $pwm1 $pwm2 (255-$p1_x) $p1_y
  # Servo_set {ServoArray} {motorPower} {objective}
  Servo_set SteeringState $pwm3 $p1_x
  Servo_set FlipperState $pwm6 $p2_x
  set turnrate [expr ($sensor2-98)*($pwm2-$pwm1)/80]
  set sensor1 [expr 127+$turnrate]
  puts "turnrate=$turnrate"
  Servo_set Heading [expr 127+$turnrate] $turnrate
  # OutputLeds_set {OutputLedsArray} {outvalue}
  OutputLeds_set OILedsArray $outword
  # Linear_set {LinearArray} {motorPower}
  Linear_set StackerLF $pwm4
  Linear_set StackerLR $pwm4
  Linear_set StackerRF $pwm5
  Linear_set StackerRR $pwm5
#puts "pwm6=$pwm6 p2_x=$p2_x sensor3=$sensor3"
  puts $logfileID "$p1_x $p1_y" ;# $tractL $tractR
  flush $logfileID
  SimInterface_set simIntArray
  set cycle [expr $cycle + 1]
}; # updatePos


#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
#@@@@@@@@@@@@@@@@@@@@@@@@@@@@ Main @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

set executable "./zipties"

# Parse the command line
while { "" != $argv }\
{
  #puts "argv='$argc'"
  if [regexp {^-e ([-_\.\\/,a-zA-Z0-9]+)(.*)} $argv dummy executable argv] \
  {
  } else {
    regexp {^([^ ]+) *} $argv dummy token
    regexp {^[^ ]+ *(.*)} $argv {} argv
    puts "Unrecognized option: '$token'."
    incr errors
  }
}; # while { "" != $argv }


#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
# The Robot

  set posCanvasX 10
  set posCanvasY 0

  # Tractor_new {Tractorname} {X} {Y} {Xs} {Ys} \
  #		       {tractInL} {tractInR} {BackColor} {Comment}
  set Tractor [Tractor_new ./tractorView $posCanvasX $posCanvasY 256 256 \
		   {1} {-1} {#7f8f7f} "Rear Tractor Wheels"]
  set posCanvasX [expr $posCanvasX + 270]

  # Linear_new {Arrayname} {X} {Y} {Xs} {Ys} {BackColor}
  #		      {MotorScale} {Inertia} {Comment}
  #		      {sw1v} {sw1b} {sw1l} {sw1u} {sw1c}
  #		      {sw2v} {sw2b} {sw2l} {sw2u} {sw2c}
  #		      {sw3v} {sw3b} {sw3l} {sw3u} {sw3c}
  #		      {sw4v} {sw4b} {sw4l} {sw4u} {sw4c}
  set dummy 0
  Linear_new StackerLR $posCanvasX [expr $posCanvasY] 40 250 {#7fafaf} \
    40 20 "StckLR" \
    rc_swA 3 0 10 "A.b3"    rc_swA 1 70 80 "A.b1" \
    rc_swA 5 90 100 "A.b5"  dummy 0 101 101 ""
  set posCanvasX [expr $posCanvasX + 50]
  Linear_new StackerLF $posCanvasX [expr $posCanvasY] 40 250 {#7fafaf} \
    40 20 "StckLF" \
    rc_swA 2 0 10 "A.b2"    rc_swA 0 70 80 "A.b0" \
    rc_swA 4 90 100 "A.b4"  dummy 0 101 101 ""
  set posCanvasX [expr $posCanvasX + 50]
  Linear_new StackerRR $posCanvasX [expr $posCanvasY] 40 250 {#7fafaf} \
    -40 20 "StckRR" \
    rc_swA 7 0 10 "A.b7"    rc_swB 4 70 80 "B.b4" \
    rc_swB 5 90 100 "B.b5"  dummy 0 101 101 ""
  set posCanvasX [expr $posCanvasX + 50]
  Linear_new StackerRF $posCanvasX [expr $posCanvasY] 40 250 {#7fafaf} \
    -40 20 "StckRF" \
    rc_swA 6 0 10 "A.b6"    rc_swB 7 70 80 "B.b7" \
    rc_swB 7 90 100 "B.b7"  dummy 0 101 101 ""
  set posCanvasX [expr $posCanvasX + 50]

  set posCanvasX 5
  set posCanvasY [expr $posCanvasY + 265]

  # Servo_new {Arrayname} {X} {Y} {Xs} {Ys} {BackColor} {servoFeedB} \
  #			{MotorScale} {servoDisDir} {servoOffset} {Comment}
  Servo_new SteeringState $posCanvasX $posCanvasY 130 127 \
      {#7f9f7f} sensor2 -20 -1.0 23.0 "Steering Servo"
  set posCanvasX [expr $posCanvasX + 130]

  Servo_new FlipperState $posCanvasX $posCanvasY 130 127 \
      {#7faf7f} sensor3 10 -1.0 0.0 "Flipper Servo"
  Servo_new Heading [expr $posCanvasX + 140] [expr $posCanvasY + 20] 115 115 \
      {#af7f7f} heading 50.0 1.0 0.0 "Heading"

  set posSwitchX [expr $posCanvasX + 140]
  set posSwitchY [expr $posCanvasY -2]

  set posCanvasX [expr $posCanvasX + 140]
  set posCanvasY [expr $posCanvasY + 140]

# ============= Switches Layout =================================

  radiobutton ./autoleft -text {} -variable autocont -value 2
  place ./autoleft -x $posSwitchX -y $posSwitchY
  set posSwitchX [expr $posSwitchX + 16]
  radiobutton ./autooff -text {} -variable autocont -value 0 -indicatoron 1
  place ./autooff -x $posSwitchX -y $posSwitchY
  set posSwitchX [expr $posSwitchX + 16]
  radiobutton ./autoright -text {autodir} -variable autocont -value 1
  place ./autoright -x $posSwitchX -y $posSwitchY
  set posSwitchX [expr $posSwitchX + 16]
  set autocont 0

  checkbutton ./autoprog1 -padx 7 -pady 0 -borderwidth 3 \
    -background {#d8d8d8} -activebackground {#f0f0f0} \
    -text {} -variable autoprog1 -offvalue 0 -onvalue 8
  place ./autoprog1 -x [expr $posSwitchX+50] -y [expr $posSwitchY-0]
  checkbutton ./autoprog0 -padx 7 -pady 0 -borderwidth 3 \
    -background {#d8d8d8} -activebackground {#f0f0f0} \
    -text {autoprog} -variable autoprog0 -offvalue 0 -onvalue 4
  place ./autoprog0 -x [expr $posSwitchX+72] -y [expr $posSwitchY-0]
  set autoprog1 0
  set autoprog0 0

#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
# Operator Interface

  canvas ./operatorInterface -borderwidth 3 -background {#7f6f7f} \
    -height 126 -width 395
  place ./operatorInterface -x 2 -y [expr $posCanvasY + 0]

  set posJoysX 10
  set posJoysY [expr $posCanvasY -5]

  # Joystick_new {posJoysX} {posJoysY} {joyX} {joyY} {joy_trigger} {joy_top}
  Joystick_new $posJoysX $posJoysY {p1_x} {p1_y} {joy_trig} {joy_top}

# OutputLeds_new {Arrayname} {X} {Y} {Vertical} {BackColor}
#  OutputLeds_new OILedsArray 5 [expr $posCanvasY + 190] 0 {#7f6f7f}
  OutputLeds_new OILedsArray 400 310 1 {#7f6f7f}

# ============= Pot Layout =================================

  set posPotsX 205
  set posPotsY 405

  # ---- p2_y ------------
  scale ./p2_y -label {p2_y Steer Mult} -variable {p2_y} -from {254} -to {1} \
    -orient horizontal
  place ./p2_y -x [expr $posPotsX] -y [expr $posPotsY + 0]
#  set posPotsY [expr $posPotsY + 62]
  # ---- P2_X ------------
  scale ./p2_x -label {p2_x Flipper} -variable {p2_x} -from {254} -to {1} \
    -orient horizontal
  place ./p2_x -x [expr $posPotsX] -y [expr $posPotsY + 62]
#  set posPotsY [expr $posPotsY + 62]
  set posPotsX [expr $posPotsX + 110]

# ============= Switches Layout =================================

  radiobutton ./stackup -text {} -variable stackcont -value 2
  place ./stackup -x $posPotsX -y $posPotsY
  set posPotsY [expr $posPotsY + 16]
  radiobutton ./stackoff -text {stacker} -variable stackcont -value 0 -indicatoron 1
  place ./stackoff -x $posPotsX -y $posPotsY
  set posPotsY [expr $posPotsY + 16]
  radiobutton ./stackdown -text {} -variable stackcont -value 1
  place ./stackdown -x $posPotsX -y $posPotsY
  set posPotsY [expr $posPotsY + 16]
  set stackcont 0

#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

  # SimInterface_new {arrayname} {objectname} {xpos} {ypos}
  SimInterface_new "simIntArray" ./simInt 2 535

# Start The Simulation

  set logfileID [open "logfile" w]
  set pipeID [open "|$executable" w+]

  puts $pipeID "0 0 0 0 127 127 0 127 127 127 127 127 127 127 0"
  updatePos
    
# ----------------------------------------------