## 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 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)
##






package require piio


##
## 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.
................................................................................
    gryo,y,axis 0x16
    gryo,z,axis 0x18
    rate 50
    freq 10
}

set i2cbus 1
set dIMUaddress {}
set range 2000dps

proc writeRegister {handle register value} {
    global Registers

    lassign $Registers($register) page addr type mask






    twowrite writeregbye $handle [lindex $Registers(page) 1] $page

    if {$mask eq {}} {
        set writeValue $value
    } else {
        ##
        ## Masked values are always byte oriented

        ##

        set writeValue [twowire readregbyte $handle $addr]
        set writeValue [format {%8.8b} $writeValue]
        lassign [split $mask] highBit lowBit
        set size [expr {1 + ($highBit - $lowBit)}]
        set tmp [format "%${size}.${size}b" $value]
        set writeValue [string replace $writeValue end-$hightBit end-$lowBit $tmp]
        scan $writeValue {%8.8b} writeValue
    }

    switch -exact -- $type {
        b {
            twowire writeregbyte $handle $addr $writeValue
        }
        w {
            twowire writeregword $handle $addr $writeValue
        }
    }

    return;
}

proc readRegister {handle register} {
    global Registers

    lassign $Registers($register) page addr type mask

    set result {}

    twowrite writeregbye $handle [lindex $Registers(page) 1] $page
    
    switch -exact -- $type {
        b {
            twowire readregbyte $handle $addr
        }
        w {
            twowire readregword $handle $addr
        }
    }

    if {$mask ne {}} {
        lassign [split $mask {:}] startbit stopbit
        set result [string range [format {%16.16b} $result] end-$startbit end-$stopbit]
        scan $result {%b} result
    }

    return $result
}

proc StartGyro {handle range} {
    global GyroValues

    set base $GyroValues(gryo,offset)

    ##
    ## Write CTRL_REG1 - Enable all axes. Disable power down.

    ##
    twowire writeregbyte $handle [expr {$base + 0x20}] 0x0F






    piio usleep 10

    ##
    ## Write CTRL_REG2 - No High Pass Filter.
    ##
    twowire writeregbyte $handle [expr {$base + 0x21}] 0x00


    piio usleep 10

    ##
    ## Write CTRL_REG3 - No interrupts.  Date ready.
    ##
    twowire writeregbyte $handle [expr {$base + 0x22}] 0x08



    piio usleep 10

    ##
    ## Write CTRL_REG4 - Specified scale range.
    ##
    twowire writeregbyte $handle [expr {$base + 0x23}] $GyroValues($range)
    piio usleep 10

    ##
    ## Write CTRL_REG5 - Enable all axes. Disable power down.
    ##
    twowire writeregbyte $handle [expr {$base + 0x24}] 0x00
    set status [twowire readbyte $handle]

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

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

    set base $GyroValues(gryo,offset)

    set intValue [twowire readregword $GyroValues(handle) [expr {$base + $GyroValues(gryo,$axis,axis)}]]
    piio usleep 10
    return [format {%8.3f} [expr {$intValue / $GyroValues(divisor)}]]
}

proc ClearHistory {} {
    global CurrentValue

................................................................................

    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
    set GyroValues(count) 0
    ReadGyro
    LogValues

}

proc StopReading {} {
    global GyroValues
................................................................................
        -textvariable ::i2cbus \
        -values [list 1] \
        -state readonly
    ttk::label $w.addrLbl \
        -text {iDMU Address:}
    ttk::entry $w.addrEnt \
        -width 5 \
        -textvariable ::dIMUaddress

    ttk::label $w.rangeLbl \
        -text {Range:}
    ttk::combobox $w.rangeCMB \
        -textvariable ::range \
        -values [list 250dps 500dps 2000dps] \
        -state readonly
    ttk::label $w.rateLbl1 \

## 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 piio
package require bno055

##
## 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.
................................................................................
    gryo,y,axis 0x16
    gryo,z,axis 0x18
    rate 50
    freq 10
}

set i2cbus 1
set dIMUaddress 0x28
set range 2000dps

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 Gryo Only mode.
    ##
    bno055 writeRegister $handle operMode [bno055 getValue OPER_MODE_GYROONLY]
    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)

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

    set handle $GyroValues(handle)
    set intValue [bno055 readRegister $handle grv,data,$axis]

    piio usleep 10
    return [format {%8.3f} [expr {$intValue / $GyroValues(divisor)}]]
}

proc ClearHistory {} {
    global CurrentValue

................................................................................

    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
    LogValues

}

proc StopReading {} {
    global GyroValues
................................................................................
        -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 \

##
## This sensor can appear at the following addresses:
##  I2C:        0x29 (primary) or 0x28 (alternate)
##  HID-I2C:    0x40
##
## Please see sction 4.6 of the Data Sheet for details of selecting the address.
##

package require piio

namespace eval bno055 {
    ##
    ## Register definition format: page address b(yte)|w(word) ?highBit:lowBit?
    ## least significant bit is 0.
    ##
................................................................................
    ##
    ## NOTE -- no checks are preformed to see if the device is in the correct
    ##         mode to read or write a perticular register. Accessing a register
    ##         when the device is not in the correct mode may result in either
    ##         an error or garbage being return.
    ##
    ## NOTE -- in the Data Sheet, PAGE_ID is colored as read only but the textual
    #          description in section 4.2 explicitly states it is used to switch
    #          between pages.
    ##
    array set Registers {
        main,chipId     {0 0x00 b}
        acc,chipId      {0 0x01 b}
        mag,chipId      {0 0x02 b}
        gyro,chipId     {0 0x03 b}
        sw,rev          {0 0x04 w}
................................................................................
        gyr,selftest    {0 0x36 b 2:2}
        mcu,selftest    {0 0x36 b 3:3}
        gyr,anymotion   {0 0x37 b 2:2}
        gyr,highrate    {0 0x37 b 3:3}
        acc,highG       {0 0x37 b 5:5}
        acc,anymotion   {0 0x37 b 6:6}
        acc,nomotion    {0 0x37 b 7:7}
        sys,clk,status  (0 0x38 b 0:0)
        sys,status      {0 0x39 b}
        sys,error       (0 0x3a b)
        acc,units       {0 0x3b b 0:0}
        gyr,units       {0 0x3b b 1:1}
        eul,units       {0 0x3b b 2:2}
        temp,units      {0 0x3b b 4:4}
        format,opt      {0 0x3b b 7:7}
        operMode        {0 0x3d b 3:0}
        powerMode       {0 0x3e b 1:0}
................................................................................
        SYS_ERROR,9     {Fusion algorithm configuration error}
        SYS_ERROR,10    {Sensor configuration error}
    }

    proc SymDef {type symbolic value} {
        variable Constants


        set Constants($type,$value) ${type}_${symbolic}
        set Constants(CONST,${type}_${symbolic}) $value
    }

    SymDef ORI_MODE WINDOWS 0
    SymDef ORI_MODE ANDROID 1
    SymDef TEMP_UNIT C 0
    SymDef TEMP_UNIT F 1
................................................................................
    SymDef TEMP_SRC ACC 0
    SymDef TEMP_SRC GRY 1
    SymDef REMAP_TO X 0b00
    SymDef REMAP_TO Y 0b01
    SymDef REMAP_TO Z 0b10
    SymDef REMAP_SIGN POS 0
    SymDef REMAP_SIGN NEG 1
    SymDef ACC_RANGE 2G 0b00B
    SymDef ACC_RANGE 4G 0b01B
    SymDef ACC_RANGE 8G 0b10B
    SymDef ACC_RANGE 16G 0b11B
    SymDef ACC_BANDWIDTH 7.81HZ 0b000
    SymDef ACC_BANDWIDTH 15.63HZ 0b001
    SymDef ACC_BANDWIDTH 31.25HZ 0b010
    SymDef ACC_BANDWIDTH 62.5HZ 0b011
    SymDef ACC_BANDWIDTH 125HZ 0b100
    SymDef ACC_BANDWIDTH 250HZ 0b101
    SymDef ACC_BANDWIDTH 500HZ 0b110
................................................................................
    SymDef GYR_AWAKE_DURR 16SAMPLES 1
    SymDef GYR_AWAKE_DURR 32SAMPLES 2
    SymDef GYR_AWAKE_DURR 64SAMPLES 3

    proc getValue {symbolic} {
        variable Constants

        return $Constants(symbol,$symbolic)






    }

    proc getSymbol {type value} {
        variable Constants


        return $Constants($type,$value)
    }

    proc writeRegister {handle register value} {
        global Registers

        lassign $Registers($register) page addr type mask

        twowrite writeregbye $handle [lindex $Registers(page) 0] $page

        if {$mask eq {}} {
            set writeValue $value
        } else {
            ##
            ## Masked values are always byte oriented
            ##
            set writeValue [twowire readregbyte $handle $addr]
            set writeValue [format {%16.16b} $writeValue]
            lassign [split $mask] highBit lowBit
            set size [expr {1 + ($highBit - $lowBit)}]
            set tmp [format "%${size}.${size}b" $value]
            set writeValue [string replace $writeValue end-$hightBit end-$lowBit $tmp]
            scan $writeValue {%b} writeValue
        }

        switch -exact -- $type {
            b {
                twowire writeregbyte $handle $addr $writeValue
            }
            w {
                twowire writeregword $handle $addr $writeValue
            }
            m {
                ##
                ## Not supported in write mode
                ##
                throw {UNSUP WRTMODE} "Multibyte writes are not supported at this time"
            }
        }

        return;
    }

    proc writeRegisterBlock {handle args} {
        global Registers

        ##
        ## Verify we need to do something
        ##
        if {[llength $args]} {
            return
        } elseif {[llength $args] == 2} {
................................................................................
        
        ##
        ## Build the value to write
        ##
        set writeValue [string repeat 0 [expr {$size * 8}]]
        foreach registerInfo $detailList {
            lassign $registerInfo page address type mask value
            lassign [split $mask] highBit lowBit
            if {$address ne $addr} {
                incr highBit 8
                incr lowBit 8
            }
            set size [expr {1 + ($highBit - $lowBit)}]
            set tmp [format "%${size}.${size}b" $value]
            set writeValue [string replace $writeValue end-$hightBit end-$lowBit $tmp]
        }
        scan $writeValue {%b} writeValue

        twowrite writeregbye $handle [lindex $Registers(page) 0] $page

        switch -exact -- $size {
            1 {
                twowire writeregbyte $handle $addr $writeValue
            }
            2 {
                twowire writeregword $handle $addr $writeValue
            }
        }

        return;
    }

    proc readRegister {handle register} {
        global Registers

        lassign $Registers($register) page addr type mask

        set result {}

        twowrite writeregbye $handle [lindex $Registers(page) 0] $page

        switch -exact -- $type {
            b {
                set result [twowire readregbyte $handle $addr]
            }
            w {
                set result [twowire readregword $handle $addr]
            }
            m {
              for {set cnt 1} {$cnt <= $mask} {incr cnt; incr addr} {
                append result [format {%2.2x} [twowire readregbyte $handle $addr]]
              } 
            }
        }

        if {$mask ne {} && $type ne {m}} {
            lassign [split $mask {:}] startbit stopbit
            set result [string range [format {%16.16b} $result] end-$startbit end-$stopbit]
            scan $result {%b} result
        }

        return $result
    }


    namespace ensemble create -subcommands {
        getValue

        getSymbol
        readRegister
        writeRegister
        writeRegisterBlock
    }

}

##
## This sensor can appear at the following addresses:
##  I2C:        0x29 (primary) or 0x28 (alternate)
##  HID-I2C:    0x40
##
## Please see sction 4.6 of the Data Sheet for details of selecting the address.
##
package provide bno055 1.0.0
package require piio

namespace eval bno055 {
    ##
    ## Register definition format: page address b(yte)|w(word) ?highBit:lowBit?
    ## least significant bit is 0.
    ##
................................................................................
    ##
    ## NOTE -- no checks are preformed to see if the device is in the correct
    ##         mode to read or write a perticular register. Accessing a register
    ##         when the device is not in the correct mode may result in either
    ##         an error or garbage being return.
    ##
    ## NOTE -- in the Data Sheet, PAGE_ID is colored as read only but the textual
    ##         description in section 4.2 explicitly states it is used to switch
    ##         between pages.
    ##
    array set Registers {
        main,chipId     {0 0x00 b}
        acc,chipId      {0 0x01 b}
        mag,chipId      {0 0x02 b}
        gyro,chipId     {0 0x03 b}
        sw,rev          {0 0x04 w}
................................................................................
        gyr,selftest    {0 0x36 b 2:2}
        mcu,selftest    {0 0x36 b 3:3}
        gyr,anymotion   {0 0x37 b 2:2}
        gyr,highrate    {0 0x37 b 3:3}
        acc,highG       {0 0x37 b 5:5}
        acc,anymotion   {0 0x37 b 6:6}
        acc,nomotion    {0 0x37 b 7:7}
        sys,clk,status  {0 0x38 b 0:0}
        sys,status      {0 0x39 b}
        sys,error       {0 0x3a b}
        acc,units       {0 0x3b b 0:0}
        gyr,units       {0 0x3b b 1:1}
        eul,units       {0 0x3b b 2:2}
        temp,units      {0 0x3b b 4:4}
        format,opt      {0 0x3b b 7:7}
        operMode        {0 0x3d b 3:0}
        powerMode       {0 0x3e b 1:0}
................................................................................
        SYS_ERROR,9     {Fusion algorithm configuration error}
        SYS_ERROR,10    {Sensor configuration error}
    }

    proc SymDef {type symbolic value} {
        variable Constants

        set binaryValue [format %b $value]
        set Constants($type,$binaryValue) ${type}_${symbolic}
        set Constants(CONST,${type}_${symbolic}) $value
    }

    SymDef ORI_MODE WINDOWS 0
    SymDef ORI_MODE ANDROID 1
    SymDef TEMP_UNIT C 0
    SymDef TEMP_UNIT F 1
................................................................................
    SymDef TEMP_SRC ACC 0
    SymDef TEMP_SRC GRY 1
    SymDef REMAP_TO X 0b00
    SymDef REMAP_TO Y 0b01
    SymDef REMAP_TO Z 0b10
    SymDef REMAP_SIGN POS 0
    SymDef REMAP_SIGN NEG 1
    SymDef ACC_RANGE 2G 0b00
    SymDef ACC_RANGE 4G 0b01
    SymDef ACC_RANGE 8G 0b10
    SymDef ACC_RANGE 16G 0b11
    SymDef ACC_BANDWIDTH 7.81HZ 0b000
    SymDef ACC_BANDWIDTH 15.63HZ 0b001
    SymDef ACC_BANDWIDTH 31.25HZ 0b010
    SymDef ACC_BANDWIDTH 62.5HZ 0b011
    SymDef ACC_BANDWIDTH 125HZ 0b100
    SymDef ACC_BANDWIDTH 250HZ 0b101
    SymDef ACC_BANDWIDTH 500HZ 0b110
................................................................................
    SymDef GYR_AWAKE_DURR 16SAMPLES 1
    SymDef GYR_AWAKE_DURR 32SAMPLES 2
    SymDef GYR_AWAKE_DURR 64SAMPLES 3

    proc getValue {symbolic} {
        variable Constants

        return $Constants(CONST,$symbolic)
    }

    proc getValue2 {type symbol} {
        variable Constants

        return $Constants(CONST,${type}_${symbol})
    }

    proc getSymbol {type value} {
        variable Constants

        set binaryValue [format %b $value]
        return $Constants($type,$binaryValue)
    }

    proc writeRegister {handle register value} {
        variable Registers

        lassign $Registers($register) page addr type mask

        ::twowire writeregbyte $handle [lindex $Registers(page) 0] $page

        if {$mask eq {}} {
            set writeValue $value
        } else {
            ##
            ## Masked values are always byte oriented
            ##
            set writeValue [::twowire readregbyte $handle $addr]
            set writeValue [format {%16.16b} $writeValue]
            lassign [split $mask {:}] highBit lowBit
            set size [expr {1 + ($highBit - $lowBit)}]
            set tmp [format "%${size}.${size}b" $value]
            set writeValue [string replace $writeValue end-$highBit end-$lowBit $tmp]
            scan $writeValue {%b} writeValue
        }

        switch -exact -- $type {
            b {
                ::twowire writeregbyte $handle $addr $writeValue
            }
            w {
                ::twowire writeregword $handle $addr $writeValue
            }
            m {
                ##
                ## Not supported in write mode
                ##
                throw {UNSUP WRTMODE} "Multibyte writes are not supported at this time"
            }
        }

        return;
    }

    proc writeRegisterBlock {handle args} {
        variable Registers

        ##
        ## Verify we need to do something
        ##
        if {[llength $args]} {
            return
        } elseif {[llength $args] == 2} {
................................................................................
        
        ##
        ## Build the value to write
        ##
        set writeValue [string repeat 0 [expr {$size * 8}]]
        foreach registerInfo $detailList {
            lassign $registerInfo page address type mask value
            lassign [split $mask {:}] highBit lowBit
            if {$address ne $addr} {
                incr highBit 8
                incr lowBit 8
            }
            set size [expr {1 + ($highBit - $lowBit)}]
            set tmp [format "%${size}.${size}b" $value]
            set writeValue [string replace $writeValue end-$highBit end-$lowBit $tmp]
        }
        scan $writeValue {%b} writeValue

        ::twowire writeregbyte $handle [lindex $Registers(page) 0] $page

        switch -exact -- $size {
            1 {
                ::twowire writeregbyte $handle $addr $writeValue
            }
            2 {
                ::twowire writeregword $handle $addr $writeValue
            }
        }

        return;
    }

    proc readRegister {handle register} {
        variable Registers

        lassign $Registers($register) page addr type mask

        set result {}

        ::twowire writeregbyte $handle [lindex $Registers(page) 0] $page

        switch -exact -- $type {
            b {
                set result [::twowire readregbyte $handle $addr]
            }
            w {
                set result [::twowire readregword $handle $addr]
            }
            m {
              for {set cnt 1} {$cnt <= $mask} {incr cnt; incr addr} {
                append result [format {%2.2x} [::twowire readregbyte $handle $addr]]
              } 
            }
        }

        if {$mask ne {} && $type ne {m}} {
            lassign [split $mask {:}] highBit lowBit
            set result [string range [format {%16.16b} $result] end-$lowBit end-$highBit]
            scan $result {%b} result
        }

        return $result
    }


    namespace ensemble create -subcommands {
        getValue
        getValue2
        getSymbol
        readRegister
        writeRegister
        writeRegisterBlock
    }

}