#!/bin/sh
# the next line restarts using wish \
exec wish "$0" ${1+"$@"}

##
## Name:  Gyroscope Example
## Author:  Gerald W. Lester, 2018
## Copyright 2018, Gerald W. Lester
##
## This program utilizes a BSD copyright, please see the license file in the
## same directory.
##
## This program is loosely based on a C program by the same name from Dexter
## Industries. See
##   https://github.com/DexterInd/DI_LEGO_NXT/blob/master/dIMU/RobotC/Gryo_Example.c
##
## It utilizes the low leve bno055 package.
##

lappend atuo_path [file dirname [info script]] [pwd]
::tcl::tm::path add [file dirname [info script]] [pwd]

package require dimu

##
## These bytes set the full scale range of the gyroscope.
## it is important to define full_scale_range.  Values are:
##      0x00 - 250 dps.  Full scale range.
##      0x10 - 500 dps.  Full scale range.
##      0x30 - 2000 dps.  Full scale range.
##
array set GyroValues {
    250dps  0x00
    500dps  0x10
    2000dps 0x30
    250dps,divisor 128.0
    500dps,divisor 64.0
    2000dps,divisor 16.0
    divisor 16.0
    full_scale_range 0x30
    gryo,x,axis 0x14
    gryo,y,axis 0x16
    gryo,z,axis 0x18
    rate 50
    digits 5
    decimals 1
    minChange 0.1
}

set i2cbus 1
set dIMUaddress 0x28
set range 2000dps


set CurrentValue(x) {0}
set CurrentValue(y) {0}
set CurrentValue(z) {0}

proc StartGyro {handle range} {
    global GyroValues

    ##
    ## Place the dIMU in configuration mode
    ##
    bno055 writeRegister $handle operMode [bno055 getValue OPER_MODE_CONFIGMODE]
    piio usleep 20

    ##
    ## Disable power down.
    ##
    bno055 writeRegister $handle powerMode [bno055 getValue PWR_MODE_NORMAL]
    piio usleep 10

    ##
    ## No High Pass Filter, Enable Any Motion
    ##
    bno055 writeRegisterBlock $handle \
        gyr,am,x,enab   1 \
        gyr,am,y,enab   1 \
        gyr,am,z,enab   1 \
        gyr,am,filter   0 \
        gyr,hr,filter   0
    piio usleep 10

    ##
    ## No interrupts.
    ##
    bno055 writeRegisterBlock $handle \
        gyr,am,intEnab  0 \
        gyr,hiR,intEnab \
    piio usleep 10

    ##
    ## Specified scale range, power mode and bandwidth.
    ##
    bno055 writeRegisterBlock $handle \
        gyr,range       [bno055 getValue2 GRY_RANGE [string toupper $range]] \
        gyr,bandwidth   [bno055 getValue GRY_BANDWIDTH_32HZ] \
        gyr,pwrMode     [bno055 getValue GRY_POWER_MODE_NORMAL]
    piio usleep 10

    ##
    ## Switch to IMU mode so autocalibration happens.
    ##
    bno055 writeRegister $handle operMode [bno055 getValue OPER_MODE_IMU]
    piio usleep 20

    ##
    ## Set divisor so that the output of our gyro axis readings can be turned
    ## into scaled values.
    ##
    set GyroValues(divisor) $GyroValues($range,divisor)

    return;
}

##
## Gyro: gets a full axis reading, scaled to the full scale reading.  Returns
## in degrees per second.
##
proc GryoAxisGeading {axis oldValue} {
    global GyroValues

    set handle $GyroValues(handle)
    set intValue [bno055 readRegister $handle gyr,data,$axis]
    set digits $GyroValues(digits)
    set decimals $GyroValues(decimals)
    incr digits $decimals
    if {$decimals} {
        ##
        ## Account for the decimal point
        ##
        incr digits
    }
    
    ##
    ## Account for the sign
    ##
    incr digits
    set newValue [format "%${digits}.${decimals}f" [expr {$intValue / $GyroValues(divisor)}]]
    if {$newValue != $oldValue && abs($newValue - $oldValue) < $GyroValues(minChange)} {
        set newValue $oldValue
    }
    return $newValue
}

proc ClearHistory {} {
    global CurrentValue

    set CurrentValue(history) {}
}

proc ReadGyro {} {
    global GyroValues CurrentValue

    if {[info exists GyroValues(afterId)]} {
        after cancel $GyroValues(afterId)
    }
    set GyroValues(afterId) [after [expr {$GyroValues(rate) * 10}] ReadGyro]

    ##
    ## Read the GYROSCOPE
    ##
    set x [GryoAxisGeading x $CurrentValue(x)]
    set y [GryoAxisGeading y $CurrentValue(y)]
    set z [GryoAxisGeading z $CurrentValue(z)]
    if {$x != $CurrentValue(x) ||
        $y != $CurrentValue(y) ||
        $z != $CurrentValue(z)
    } {
        LogValues $x $y $z
    }

    set CurrentValue(x) $x
    set CurrentValue(y) $y
    set CurrentValue(z) $z

    return
}

proc LogValues {x y z} {
    global GyroValues CurrentValue

    #if {[info exists GyroValues(logId)]} {
    #    after cancel $GyroValues(logId)
    #}
    #set GyroValues(logId) [after [expr {$GyroValues(freq) * 100}] LogValues]

    set timeStamp [clock format [clock seconds]]
    .main.history.data insert {} end -values [list $timeStamp $x $y $z]
}

proc StartReading {} {
    global i2cbus dIMUaddress range GyroValues

    if {$dIMUaddress eq {}} {
        return
    }

    .main.controls.buttons.start configure -state disabled
    .main.controls.buttons.stop configure -state normal
    set GyroValues(handle) [::twowire twowire $i2cbus $dIMUaddress]
    StartGyro $GyroValues(handle) $range
    ReadGyro

}

proc StopReading {} {
    global GyroValues

    if {[info exists GyroValues(afterId)]} {
        after cancel $GyroValues(afterId)
    }
    if {[info exists GyroValues(logId)]} {
        after cancel $GyroValues(logId)
    }
    
    .main.controls.buttons.start configure -state normal 
    .main.controls.buttons.stop configure -state disabled
}

proc CreateGui {} {
    wm title . "dIMU Gyroscope Example"

    ttk::notebook .main
    grid configure .main -sticky nsew
    grid columnconfigure . .main -weight 1
    grid rowconfigure . .main -weight 1
    
    ttk::frame .main.configuration
    .main add .main.configuration -text {Configuration}
    ttk::frame .main.gyroscope
    .main add .main.gyroscope -text {Gryoscope}
    ttk::frame .main.accelerometer
    .main add .main.accelerometer -text {Accelerometer}
    ttk::frame .main.magnetometer
    .main add .main.magnetometer -text {Magnetometer}
    ttk::frame .main.gravity
    .main add .main.gravity -text {Gravity}
    ttk::frame .main.linearAcceleration
    .main add .main.linearAcceleration -text {Linear Acceleration}
    ttk::frame .main.quaternion
    .main add .main.quaternion -text {Quaternion}
    ttk::frame .main.prh
    .main add .main.prh -text {Pitch, Roll and Heading}

    CreateConfigurationFrame .main.configuration
    CreateGyroFrame .main.gyroscope
    CreateAccelerometerFrame .main.accelerometer
    CreateMagnetometerFrame .main.magnetometer
    CreateGravityFrame .main.
    CreateLinearAccelerationFrame .main.
    CreateQuaternionFrame .main.
    CreatePitchRollHeadingFrame .main.
}

proc CreateControlFrame {w} {
    ttk::label $w.busLbl \
        -text {I2C Bus:}
    ttk::combobox $w.busCMB \
        -textvariable ::i2cbus \
        -values [list 1] \
        -state readonly
    ttk::label $w.addrLbl \
        -text {iDMU Address:}
    ttk::entry $w.addrEnt \
        -width 5 \
        -textvariable ::dIMUaddress \
        -state readonly
    ttk::label $w.rangeLbl \
        -text {Range:}
    ttk::combobox $w.rangeCMB \
        -textvariable ::range \
        -values [list 250dps 500dps 2000dps] \
        -state readonly
    ttk::label $w.rateLbl1 \
        -text {Refresh Rate:}
    ttk::spinbox $w.rateEnt \
        -from 50.0 \
        -to 500.0 \
        -increment 10.0 \
        -format {%3.0f} \
        -wrap no \
        -command [format {set ::GyroValues(rate) [expr {entier([%1$s get])}]}  $w.rateEnt]
    $w.rateEnt set 50.0
    ttk::label $w.rateLbl2 \
        -text { in microseconds}
    ttk::label $w.numDigitsLbl \
        -text {Digits before decimal}
    ttk::combobox $w.numDigitsCMB \
        -textvariable ::GyroValues(digits) \
        -values [list 3 4] \
        -state readonly
    ttk::label $w.numDecimalsLbl \
        -text {Digits after decimal}
    ttk::combobox $w.numDecimalsCMB \
        -textvariable ::GyroValues(decimals) \
        -values [list 1 2 3 4] \
        -state readonly
    ttk::label $w.minChangeLbl1 \
        -text {Ignore less than}
    ttk::spinbox $w.minChangeEnt \
        -from 0.1 \
        -to 200.0 \
        -increment 0.1 \
        -format {%5.1f} \
        -wrap no \
        -textvariable ::GyroValues(minChange)
    ttk::label $w.minChangeLbl2 \
        -text {dps change}
    ttk::frame $w.buttons
    ttk::button $w.buttons.start \
        -text {Start} \
        -width 6 \
        -command StartReading
    ttk::button $w.buttons.stop \
        -state disabled \
        -text {Stop} \
        -width 6 \
        -command StopReading
    ttk::button $w.buttons.reset \
        -text {Clear Log} \
        -width 10 \
        -command {.main.history.data delete [.main.history.data children {}]}
    
    $w.minChangeEnt set $::GyroValues(minChange)
        
    grid configure $w.busLbl $w.busCMB $w.addrLbl $w.addrEnt $w.rangeLbl $w.rangeCMB x -padx 2 -sticky ew
    grid configure $w.rateLbl1 $w.rateEnt $w.rateLbl2 x x x x -padx 2 -sticky ew
    grid configure $w.numDigitsLbl $w.numDigitsCMB $w.numDecimalsLbl $w.numDecimalsCMB x x x -padx 2 -sticky ew
    grid configure $w.minChangeLbl1 $w.minChangeEnt $w.minChangeLbl2 x x x x -padx 2 -sticky ew
    grid configure $w.buttons -columnspan 7 -sticky ew -pady 3
    grid configure x $w.buttons.start x  $w.buttons.stop x  $w.buttons.reset x -sticky ew
    grid columnconfigure $w.buttons {0 2 4 6} -weight 1
    grid columnconfigure $w 6 -weight 1
        
}

proc CreateCurrentReadingsFrame {w} {
    ttk::label $w.xLabel \
        -text {X:}
    ttk::entry $w.xValue \
        -textvariable ::CurrentValue(x) \
        -state readonly
    ttk::label $w.yLabel \
        -text {Y:}
    ttk::entry $w.yValue \
        -textvariable ::CurrentValue(y) \
        -state readonly
    ttk::label $w.zLabel \
        -text {Z:}
    ttk::entry $w.zValue \
        -textvariable ::CurrentValue(z) \
        -state readonly
    grid configure $w.xLabel $w.xValue $w.yLabel $w.yValue $w.zLabel $w.zValue x -sticky ew -padx 2 -pady 1
    grid columnconfigure $w [list $w.xLabel $w.yLabel $w.zLabel] -uniform labels -weight 0
    grid columnconfigure $w [list $w.xValue $w.yValue $w.zValue] -uniform values -weight 0
    grid columnconfigure $w 6 -weight 1
}

proc CreateHistoryFrame {w} {
    ttk::treeview $w.data \
        -xscrollcommand [list $w.hsb set] \
        -yscrollcommand [list $w.vsb set] \
        -columns {timestamp xaxis yaxis zaxis} \
        -displaycolumns  {timestamp xaxis yaxis zaxis} \
        -show headings \
        -selectmode none
    ttk::scrollbar $w.vsb \
        -orient vertical \
        -command [list $w.data yview]
    ttk::scrollbar $w.hsb \
        -orient horizontal \
        -command [list $w.data xview]
    
    foreach column {timestamp xaxis yaxis zaxis} heading {"Time Stamp" "X Axis" "Y Axis" "Z Axis"} {
        $w.data column $column \
            -anchor w \
            -stretch yes
        $w.data heading $column \
            -anchor center \
            -text $heading
    }
    $w.data column timestamp \
            -anchor e

    grid configure $w.data $w.vsb -sticky nsew
    grid configure $w.hsb -sticky ew
    grid columnconfigure $w $w.data -weight 1
    grid rowconfigure $w $w.data -weight 1
}

CreateGui

<?xml version="1.0" encoding="UTF-8"?>
<!-- Komodo Project File - DO NOT EDIT -->
<project id="3be2a3a6-bb09-4bd7-bb7c-2bfdb3a7e0fa" kpf_version="5" name="dMIU.komodoproject">
<preference-set idref="3be2a3a6-bb09-4bd7-bb7c-2bfdb3a7e0fa" id="project" preftype="project">
  <long id="prefs_version">1</long>
</preference-set>
</project>