[r1]: / trunk / legacy / Libraries / I2C_Gyroscopes.library.md Maximize Restore History
326 lines (275 with data), 6.1 kB
<?xml version="1.0" encoding="utf-8"?>---
FUNCTION_BLOCK MPU6050 EXTENDS i2c
VAR_OUTPUT
lrAX: LREAL;
lrAY: LREAL;
lrAZ: LREAL;
lrGX: LREAL;
lrGY: LREAL;
lrGZ: LREAL;
lrTemp: LREAL;
END_VAR
VAR
iAX: INT;
iAY: INT;
iAZ: INT;
iGX: INT;
iGY: INT;
iGZ: INT;
uiTemp: UINT;
lrOffsetGX: LREAL;
lrOffsetGY: LREAL;
lrOffsetGZ: LREAL;
iCounter: INT;
END_VAR
SUPER^();
CASE _iState OF
0:
IF usiAddress = 0 THEN
usiAddress := 16#69;
END_IF
IF init() THEN
_iState := 5;
END_IF
5:
write8(16#6B, 16#80);
_iState := 6;
6:
write8(16#6B, 16#3); //SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
writeBits(16#1B, 4, 2, 3);
writeBits(16#1C, 4, 2, 2);
write8(16#1A, 2); //0: Config 260Hz 0ms delay 4: 21Hz 8.5ms
_iState := 8;
8:
iCounter := iCounter + 1;
IF iCounter = 200 THEN
iCounter := 0;
_iState := 9;
END_IF
END_CASE
METHOD AfterReadInputs: INT
VAR
aby: ARRAY [..] OF ;
END_VAR
SUPER^.AfterReadInputs();
IF _iState = 9 OR _iState = 10 THEN
readregister(59, ADR(aby), 14);
iAX := UINT_TO_INT(aby[0] * UINT#256 + aby[1]);
iAY := UINT_TO_INT(aby[2] * UINT#256 + aby[3]);
iAZ := UINT_TO_INT(aby[4] * UINT#256 + aby[5]);
uiTemp := aby[6] * UINT#256 + aby[7];
iGX := UINT_TO_INT(aby[8] * UINT#256 + aby[9]);
iGY := UINT_TO_INT(aby[10] * UINT#256 + aby[11]);
iGZ := UINT_TO_INT(aby[12] * UINT#256 + aby[13]);
lrAX := iAX * LREAL#9.81 / LREAL#4096;
lrAY := iAY * LREAL#9.81 / LREAL#4096;
lrAZ := iAZ * LREAL#9.81 / LREAL#4096;
IF _iState = 9 THEN //calibrate
iCounter := iCounter + 1;
lrOffsetGX := lrOffsetGX + iGX * 2000 / LREAL#16384;
lrOffsetGY := lrOffsetGY + iGY * 2000 / LREAL#16384;
lrOffsetGZ := lrOffsetGZ + iGZ * 2000 / LREAL#16384;
IF iCounter = 2000 THEN
lrOffsetGX := -lrOffsetGX / iCounter;
lrOffsetGY := -lrOffsetGY / iCounter;
lrOffsetGZ := -lrOffsetGZ / iCounter;
_iState := 10;
END_IF
ELSE
lrGX := INT_TO_LREAL(iGX) * LREAL#2000 / LREAL#16384 + lrOffsetGX;
lrGY := INT_TO_LREAL(iGY) * LREAL#2000 / LREAL#16384 + lrOffsetGY;
lrGZ := INT_TO_LREAL(iGZ) * LREAL#2000 / LREAL#16384 + lrOffsetGZ;
END_IF
lrTemp := (UINT_TO_INT(uiTemp) + LREAL#12412) / LREAL#340.0;
END_IF
METHOD Calibrate:
lrOffsetGX := 0;
lrOffsetGY := 0;
lrOffsetGZ := 0;
iCounter := 0;
_iState := 9;
FUNCTION_BLOCK AK8975 EXTENDS i2c
VAR_OUTPUT
iMX: INT;
iMY: INT;
iMZ: INT;
END_VAR
VAR
aby: ARRAY [..] OF ;
timer: ton;
END_VAR
SUPER^();
CASE _iState OF
0:
IF usiAddress = 0 THEN
usiAddress := 16#0C;
END_IF
IF init() THEN
write8(16#0A, 1);
_iState := 10;
timer(IN:=TRUE);
END_IF
END_CASE
METHOD AfterReadInputs: INT
SUPER^.AfterReadInputs();
IF _iState = 10 THEN
timer();
IF timer.Q THEN
timer(IN := FALSE);
timer(IN := TRUE);
readregister(3, ADR(aby), 6);
iMX := UINT_TO_INT(aby[0]*UINT#256 + aby[1]);
iMY := UINT_TO_INT(aby[2]*UINT#256 + aby[3]);
iMZ := UINT_TO_INT(aby[4]*UINT#256 + aby[5]);
write8(16#0A, 1); //retrigger
END_IF
END_IF
FUNCTION_BLOCK MPU9150 EXTENDS i2c
VAR_OUTPUT
lrAX: LREAL;
lrAY: LREAL;
lrAZ: LREAL;
lrGX: LREAL;
lrGY: LREAL;
lrGZ: LREAL;
lrTemp: LREAL;
END_VAR
VAR
Magnet: AK8975;
iAX: INT;
iAY: INT;
iAZ: INT;
iGX: INT;
iGY: INT;
iGZ: INT;
uiTemp: UINT;
lrOffsetGX: LREAL;
lrOffsetGY: LREAL;
lrOffsetGZ: LREAL;
iCounter: INT;
END_VAR
SUPER^();
CASE _iState OF
0:
IF usiAddress = 0 THEN
usiAddress := 16#69;
END_IF
IF SUPER^.init() THEN
_iState := 5;
END_IF
5:
write8(16#6B, 16#80);
_iState := 6;
6:
write8(16#6B, 16#3); //SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
writeBits(16#1B, 4, 2, 3);
writeBits(16#1C, 4, 2, 2);
write8(16#1A, 2); //0: Config 260Hz 0ms delay 4: 21Hz 8.5ms
write8(16#37, 2);
_iState := 7;
7:
Magnet();
IF Magnet.Operational THEN
_iState := 8;
END_IF
8:
iCounter := iCounter + 1;
IF iCounter = 200 THEN
iCounter := 0;
_iState := 9;
END_IF
END_CASE
METHOD AfterReadInputs: INT
VAR
aby: ARRAY [..] OF ;
END_VAR
SUPER^.AfterReadInputs();
IF _iState = 9 OR _iState = 10 THEN
readregister(59, ADR(aby), 14);
iAX := UINT_TO_INT(aby[0] * UINT#256 + aby[1]);
iAY := UINT_TO_INT(aby[2] * UINT#256 + aby[3]);
iAZ := UINT_TO_INT(aby[4] * UINT#256 + aby[5]);
uiTemp := aby[6] * UINT#256 + aby[7];
iGX := UINT_TO_INT(aby[8] * UINT#256 + aby[9]);
iGY := UINT_TO_INT(aby[10] * UINT#256 + aby[11]);
iGZ := UINT_TO_INT(aby[12] * UINT#256 + aby[13]);
lrAX := iAX * LREAL#9.81 / LREAL#4096;
lrAY := iAY * LREAL#9.81 / LREAL#4096;
lrAZ := iAZ * LREAL#9.81 / LREAL#4096;
IF _iState = 9 THEN //calibrate
iCounter := iCounter + 1;
lrOffsetGX := lrOffsetGX + iGX;
lrOffsetGY := lrOffsetGY + iGY;
lrOffsetGZ := lrOffsetGZ + iGZ;
IF iCounter = 2000 THEN
lrOffsetGX := -lrOffsetGX / iCounter;
lrOffsetGY := -lrOffsetGY / iCounter;
lrOffsetGZ := -lrOffsetGZ / iCounter;
_iState := 10;
END_IF
ELSE
lrGX := (INT_TO_LREAL(iGX) + lrOffsetGX) / 16.4;
lrGY := (INT_TO_LREAL(iGY) + lrOffsetGY) / 16.4;
lrGZ := (INT_TO_LREAL(iGZ) + lrOffsetGZ) / 16.4;
END_IF
lrTemp := (UINT_TO_INT(uiTemp) + LREAL#12412) / LREAL#340.0;
END_IF
Magnet.AfterReadInputs();
METHOD Calibrate:
lrOffsetGX := 0;
lrOffsetGY := 0;
lrOffsetGZ := 0;
iCounter := 0;
_iState := 9;
METHOD Initialize: UDINT
VAR_INPUT
wModuleType: UINT;
dwInstance: UDINT;
pConnector: pointer;
END_VAR
SUPER^.Initialize(wModuleType, dwInstance, pConnector);
Magnet.Initialize(wModuleType, dwInstance, pConnector);