tinygo.org/x/drivers@v0.27.1-0.20240509133757-7dbca2a54349/examples/lsm6dsox/main.go (about)

     1  // Connects to an LSM6DSOX I2C a 6 axis Inertial Measurement Unit (IMU)
     2  package main
     3  
     4  import (
     5  	"fmt"
     6  	"machine"
     7  	"time"
     8  
     9  	"tinygo.org/x/drivers/lsm6dsox"
    10  )
    11  
    12  const (
    13  	PLOTTER           = true
    14  	SHOW_ACCELERATION = false
    15  	SHOW_ROTATION     = true
    16  	SHOW_TEMPERATURE  = false
    17  
    18  	// calibration is naive, good enough for a demo: do not shake a second after flashing
    19  	GYRO_CALIBRATION = true
    20  )
    21  
    22  var cal = [...]float32{0, 0, 0}
    23  
    24  func main() {
    25  
    26  	machine.I2C0.Configure(machine.I2CConfig{})
    27  
    28  	device := lsm6dsox.New(machine.I2C0)
    29  	err := device.Configure(lsm6dsox.Configuration{
    30  		AccelRange:      lsm6dsox.ACCEL_2G,
    31  		AccelSampleRate: lsm6dsox.ACCEL_SR_104,
    32  		GyroRange:       lsm6dsox.GYRO_250DPS,
    33  		GyroSampleRate:  lsm6dsox.GYRO_SR_104,
    34  	})
    35  	if err != nil {
    36  		for {
    37  			println("Failed to configure", err.Error())
    38  			time.Sleep(time.Second)
    39  		}
    40  	}
    41  
    42  	for {
    43  
    44  		if !device.Connected() {
    45  			println("LSM6DSOX not connected")
    46  			time.Sleep(time.Second)
    47  			continue
    48  		}
    49  
    50  		// heuristic: after successful calibration the value can't be 0
    51  		if GYRO_CALIBRATION && cal[0] == 0 {
    52  			calibrateGyro(device)
    53  		}
    54  
    55  		ax, ay, az, _ := device.ReadAcceleration()
    56  		gx, gy, gz, _ := device.ReadRotation()
    57  		t, _ := device.ReadTemperature()
    58  
    59  		if PLOTTER {
    60  			printPlotter(ax, ay, az, gx, gy, gz, t)
    61  			time.Sleep(time.Millisecond * 100)
    62  		} else {
    63  			printMonitor(ax, ay, az, gx, gy, gz, t)
    64  			time.Sleep(time.Millisecond * 1000)
    65  		}
    66  
    67  	}
    68  
    69  }
    70  
    71  func calibrateGyro(device *lsm6dsox.Device) {
    72  	for i := 0; i < 100; i++ {
    73  		gx, gy, gz, _ := device.ReadRotation()
    74  		cal[0] += float32(gx) / 1000000
    75  		cal[1] += float32(gy) / 1000000
    76  		cal[2] += float32(gz) / 1000000
    77  		time.Sleep(time.Millisecond * 10)
    78  	}
    79  	cal[0] /= 100
    80  	cal[1] /= 100
    81  	cal[2] /= 100
    82  }
    83  
    84  // Arduino IDE's Serial Plotter
    85  func printPlotter(ax, ay, az, gx, gy, gz, t int32) {
    86  	if SHOW_ACCELERATION {
    87  		fmt.Printf("AX:%f, AY:%f, AZ:%f,", axis(ax, 0), axis(ay, 0), axis(az, 0))
    88  	}
    89  	if SHOW_ROTATION {
    90  		fmt.Printf("GX:%f, GY:%f, GZ:%f,", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
    91  	}
    92  	if SHOW_TEMPERATURE {
    93  		fmt.Printf("T:%f", float32(t)/1000)
    94  	}
    95  	println()
    96  }
    97  
    98  // Any Serial Monitor
    99  func printMonitor(ax, ay, az, gx, gy, gz, t int32) {
   100  	if SHOW_ACCELERATION {
   101  		fmt.Printf("Acceleration: %f, %f, %f\r\n", axis(ax, 0), axis(ay, 0), axis(az, 0))
   102  	}
   103  	if SHOW_ROTATION {
   104  		fmt.Printf("Rotation: %f, %f, %f\r\n", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
   105  	}
   106  	if SHOW_TEMPERATURE {
   107  		fmt.Printf("Temperature C: %f\r\n", float32(t)/1000)
   108  	}
   109  	println()
   110  }
   111  
   112  func axis(raw int32, cal float32) float32 {
   113  	return float32(raw)/1000000 - cal
   114  }