| mpu6050 step mpuAddr pwrMgmt1 gyroConf delay
accelConf accelXOutH accelYOutH accelZOutH
gyroXOutH gyroYOutH gyroZOutH
accx accy accz read step
gx gy gz count degRes gRes|
mpuAddr := 104. "hex:0x68 bin:0b1101000 mpu6050 address in slave mode. See datasheet section 9.2"
pwrMgmt1 := 107. "hex: 0x6B , reset mpu PWR_MGMT_1. See section 4.28 datasheet rev 4.2"
gyroConf := 27. "hex: 0x1B, accesing GYRO_CONFIG. See section 4.4 datasheet rev 4.2"
accelConf := 28. "hex: 0x1C, accesing ACCEL_CONFIG. See section 4.5 datasheet rev 4.2"
accelXOutH := 59. "hex:0x3B regiter ACCEL_XOUT_H. See section 3 datasheet rev 4.2"
accelYOutH := 61. "hex:0x3D regiter ACCEL_YOUT_H. See section 3 datasheet rev 4.2"
accelZOutH := 63. "hex:0x3F regiter ACCEL_ZOUT_H. See section 3 datasheet rev 4.2"
gyroXOutH := 67. "hex:0x43 regiter GYRO_XOUT_H. See section 3 datasheet rev 4.2"
gyroYOutH := 69. "hex:0x45 regiter GYRO_YOUT_H. See section 3 datasheet rev 4.2"
gyroZOutH := 71. "hex:0x47 regiter GYRO_ZOUT_H. See section 3 datasheet rev 4.2"
accx := 0.
accy := 0.
accz := 0.
gx := 0.
gy := 0.
gz := 0.
delay := 100.
degRes := 131. "Degrees resolution. See section 4.19 datasheet revision 4.2"
gRes := 16384. "Gravity resolution. See section 4.17 datasheet revision 4.2"
read := [:command :div| |buf value|
buf:= NodeBuffer size: 2.
mpu6050 readI2cBlockSync: mpuAddr cmd: command length: 2 buffer: (buf handle).
value := ((buf readInt8:0 ) * 128) + (buf readInt8:1 ).
value/div.
].
step :=[
accx := read value: accelXOutH value: gRes.
accy := read value: accelYOutH value: gRes.
accz := read value: accelZOutH value: gRes.
gx := read value: gyroXOutH value: degRes.
gy := read value: gyroYOutH value: degRes.
gz := read value: gyroZOutH value: degRes.
self print: 'Gyro(deg)>> x:', (gx #toFixed:2) , ' y:', (gy #toFixed:2) , ' z:', (gz #toFixed:2),
' Accel(g)>> x:', (accx #toFixed:2) , ' y:', (accy #toFixed:2) , ' z:', (accz #toFixed:2).
step valueDeferred: delay.
].
mpu6050 := RPI i2cBus openSync:1.
mpu6050
writeByteSync: mpuAddr cmd: pwrMgmt1 byte: 0;
writeByteSync: mpuAddr cmd: gyroConf byte: 0;
writeByteSync: mpuAddr cmd: accelConf byte: 0.
step value.