姿态控制仪（arduino 及 mpu6050）
姿态控制仪的代码
// MPU-6050 Accelerometer + Gyro
// -----------------------------
//
// By arduino.cc user "Krodal".
// June 2012
// Open Source / Public Domain
// Modified by Debra - http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/
//
// Using Arduino 1.0.1
// It will not work with an older version,
// since Wire.endTransmission() uses a parameter
// to hold or release the I2C bus.
//
// Documentation:
// - The InvenSense documents:
// - "MPU-6000 and MPU-6050 Product Specification",
// PS-MPU-6000A.pdf
// - "MPU-6000 and MPU-6050 Register Map and Descriptions",
// RM-MPU-6000A.pdf or RS-MPU-6000A.pdf
// - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide"
// AN-MPU-6000EVB.pdf
//
// The accuracy is 16-bits.
//
// Temperature sensor from -40 to +85 degrees Celsius
// 340 per degrees, -512 at 35 degrees.
//
// At power-up, all registers are zero, except these two:
// Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero).
// Register 0x75 (WHO_AM_I) = 0x68.
//
#include
// the Arduino "bit(X)" is used.
#define MPU6050_D0 0
#define MPU6050_D1 1
#define MPU6050_D2 2
#define MPU6050_D3 3
#define MPU6050_D4 4
#define MPU6050_D5 5
#define MPU6050_D6 6
#define MPU6050_D7 7
// AUX_VDDIO Register
#define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC
// CONFIG Register
// DLPF is Digital Low Pass Filter for both gyro and accelerometers.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DLPF_CFG0 MPU6050_D0
#define MPU6050_DLPF_CFG1 MPU6050_D1
#define MPU6050_DLPF_CFG2 MPU6050_D2
#define MPU6050_EXT_SYNC_SET0 MPU6050_D3
#define MPU6050_EXT_SYNC_SET1 MPU6050_D4
#define MPU6050_EXT_SYNC_SET2 MPU6050_D5
// Combined definitions for the EXT_SYNC_SET values
#define MPU6050_EXT_SYNC_SET_0 (0)
#define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1))
#define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2))
#define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1))
#define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
// Alternative names for the combined definitions.
#define MPU6050_EXT_SYNC_DISABLED MPU6050_EXT_SYNC_SET_0
#define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7
// Combined definitions for the DLPF_CFG values
#define MPU6050_DLPF_CFG_0 (0)
#define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1))
#define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2))
#define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1))
#define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
// Alternative names for the combined definitions
// This name uses the bandwidth (Hz) for the accelometer,
// for the gyro the bandwidth is almost the same.
#define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0
#define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1
#define MPU6050_DLPF_94HZ MPU6050_DLPF_CFG_2
#define MPU6050_DLPF_44HZ MPU6050_DLPF_CFG_3
#define MPU6050_DLPF_21HZ MPU6050_DLPF_CFG_4
#define MPU6050_DLPF_10HZ MPU6050_DLPF_CFG_5
#define MPU6050_DLPF_5HZ MPU6050_DLPF_CFG_6
#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7
// GYRO_CONFIG Register
// The XG_ST, YG_ST, ZG_ST are bits for selftest.
// The FS_SEL sets the range for the gyro.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_FS_SEL0 MPU6050_D3
#define MPU6050_FS_SEL1 MPU6050_D4
#define MPU6050_ZG_ST MPU6050_D5
#define MPU6050_YG_ST MPU6050_D6
#define MPU6050_XG_ST MPU6050_D7
// Combined definitions for the FS_SEL values
#define MPU6050_FS_SEL_0 (0)
#define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0))
#define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1))
#define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0))
// Alternative names for the combined definitions
// The name uses the range in degrees per second.
#define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0
#define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1
#define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2
#define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3
// ACCEL_CONFIG Register
// The XA_ST, YA_ST, ZA_ST are bits for selftest.
// The AFS_SEL sets the range for the accelerometer.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_ACCEL_HPF0 MPU6050_D0
#define MPU6050_ACCEL_HPF1 MPU6050_D1
#define MPU6050_ACCEL_HPF2 MPU6050_D2
#define MPU6050_AFS_SEL0 MPU6050_D3
#define MPU6050_AFS_SEL1 MPU6050_D4
#define MPU6050_ZA_ST MPU6050_D5
#define MPU6050_YA_ST MPU6050_D6
#define MPU6050_XA_ST MPU6050_D7
// Combined definitions for the ACCEL_HPF values
#define MPU6050_ACCEL_HPF_0 (0)
#define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0))
#define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1))
#define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
#define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2))
#define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
// Alternative names for the combined definitions
// The name uses the Cut-off frequency.
#define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0
#define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1
#define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2
#define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3
#define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4
#define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7
// Combined definitions for the AFS_SEL values
#define MPU6050_AFS_SEL_0 (0)
#define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0))
#define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1))
#define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0))
// Alternative names for the combined definitions
// The name uses the full scale range for the accelerometer.
#define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0
#define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1
#define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2
#define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3
// FIFO_EN Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_SLV0_FIFO_EN MPU6050_D0
#define MPU6050_SLV1_FIFO_EN MPU6050_D1
#define MPU6050_SLV2_FIFO_EN MPU6050_D2
#define MPU6050_ACCEL_FIFO_EN MPU6050_D3
#define MPU6050_ZG_FIFO_EN MPU6050_D4
#define MPU6050_YG_FIFO_EN MPU6050_D5
#define MPU6050_XG_FIFO_EN MPU6050_D6
#define MPU6050_TEMP_FIFO_EN MPU6050_D7
// I2C_MST_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_MST_CLK0 MPU6050_D0
#define MPU6050_I2C_MST_CLK1 MPU6050_D1
#define MPU6050_I2C_MST_CLK2 MPU6050_D2
#define MPU6050_I2C_MST_CLK3 MPU6050_D3
#define MPU6050_I2C_MST_P_NSR MPU6050_D4
#define MPU6050_SLV_3_FIFO_EN MPU6050_D5
#define MPU6050_WAIT_FOR_ES MPU6050_D6
#define MPU6050_MULT_MST_EN MPU6050_D7
// Combined definitions for the I2C_MST_CLK
#define MPU6050_I2C_MST_CLK_0 (0)
#define MPU6050_I2C_MST_CLK_1 (bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_2 (bit(MPU6050_I2C_MST_CLK1))
#define MPU6050_I2C_MST_CLK_3 (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_4 (bit(MPU6050_I2C_MST_CLK2))
#define MPU6050_I2C_MST_CLK_5 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_6 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1))
#define MPU6050_I2C_MST_CLK_7 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_8 (bit(MPU6050_I2C_MST_CLK3))
#define MPU6050_I2C_MST_CLK_9 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1))
#define MPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2))
#define MPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0))
#define MPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1))
#define MPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0))
// Alternative names for the combined definitions
// The names uses I2C Master Clock Speed in kHz.
#define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0
#define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1
#define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2
#define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3
#define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4
#define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5
#define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6
#define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7
#define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8
#define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9
#define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10
#define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11
#define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12
#define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13
#define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14
#define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15
// I2C_SLV0_ADDR Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV0_RW MPU6050_D7
// I2C_SLV0_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV0_LEN0 MPU6050_D0
#define MPU6050_I2C_SLV0_LEN1 MPU6050_D1
#define MPU6050_I2C_SLV0_LEN2 MPU6050_D2
#define MPU6050_I2C_SLV0_LEN3 MPU6050_D3
#define MPU6050_I2C_SLV0_GRP MPU6050_D4
#define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5
#define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6
#define MPU6050_I2C_SLV0_EN MPU6050_D7
// A mask for the length
#define MPU6050_I2C_SLV0_LEN_MASK 0x0F
// I2C_SLV1_ADDR Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV1_RW MPU6050_D7
// I2C_SLV1_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV1_LEN0 MPU6050_D0
#define MPU6050_I2C_SLV1_LEN1 MPU6050_D1
#define MPU6050_I2C_SLV1_LEN2 MPU6050_D2
#define MPU6050_I2C_SLV1_LEN3 MPU6050_D3
#define MPU6050_I2C_SLV1_GRP MPU6050_D4
#define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5
#define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6
#define MPU6050_I2C_SLV1_EN MPU6050_D7
// A mask for the length
#define MPU6050_I2C_SLV1_LEN_MASK 0x0F
// I2C_SLV2_ADDR Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV2_RW MPU6050_D7
// I2C_SLV2_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV2_LEN0 MPU6050_D0
#define MPU6050_I2C_SLV2_LEN1 MPU6050_D1
#define MPU6050_I2C_SLV2_LEN2 MPU6050_D2
#define MPU6050_I2C_SLV2_LEN3 MPU6050_D3
#define MPU6050_I2C_SLV2_GRP MPU6050_D4
#define MPU6050_I2C_SLV2_REG_DIS MPU6050_D5
#define MPU6050_I2C_SLV2_BYTE_SW MPU6050_D6
#define MPU6050_I2C_SLV2_EN MPU6050_D7
// A mask for the length
#define MPU6050_I2C_SLV2_LEN_MASK 0x0F
// I2C_SLV3_ADDR Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV3_RW MPU6050_D7
// I2C_SLV3_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV3_LEN0 MPU6050_D0
#define MPU6050_I2C_SLV3_LEN1 MPU6050_D1
#define MPU6050_I2C_SLV3_LEN2 MPU6050_D2
#define MPU6050_I2C_SLV3_LEN3 MPU6050_D3
#define MPU6050_I2C_SLV3_GRP MPU6050_D4
#define MPU6050_I2C_SLV3_REG_DIS MPU6050_D5
#define MPU6050_I2C_SLV3_BYTE_SW MPU6050_D6
#define MPU6050_I2C_SLV3_EN MPU6050_D7
// A mask for the length
#define MPU6050_I2C_SLV3_LEN_MASK 0x0F
// I2C_SLV4_ADDR Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV4_RW MPU6050_D7
// I2C_SLV4_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_MST_DLY0 MPU6050_D0
#define MPU6050_I2C_MST_DLY1 MPU6050_D1
#define MPU6050_I2C_MST_DLY2 MPU6050_D2
#define MPU6050_I2C_MST_DLY3 MPU6050_D3
#define MPU6050_I2C_MST_DLY4 MPU6050_D4
#define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5
#define MPU6050_I2C_SLV4_INT_EN MPU6050_D6
#define MPU6050_I2C_SLV4_EN MPU6050_D7
// A mask for the delay
#define MPU6050_I2C_MST_DLY_MASK 0x1F
// I2C_MST_STATUS Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV0_NACK MPU6050_D0
#define MPU6050_I2C_SLV1_NACK MPU6050_D1
#define MPU6050_I2C_SLV2_NACK MPU6050_D2
#define MPU6050_I2C_SLV3_NACK MPU6050_D3
#define MPU6050_I2C_SLV4_NACK MPU6050_D4
#define MPU6050_I2C_LOST_ARB MPU6050_D5
#define MPU6050_I2C_SLV4_DONE MPU6050_D6
#define MPU6050_PASS_THROUGH MPU6050_D7
// I2C_PIN_CFG Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_CLKOUT_EN MPU6050_D0
#define MPU6050_I2C_BYPASS_EN MPU6050_D1
#define MPU6050_FSYNC_INT_EN MPU6050_D2
#define MPU6050_FSYNC_INT_LEVEL MPU6050_D3
#define MPU6050_INT_RD_CLEAR MPU6050_D4
#define MPU6050_LATCH_INT_EN MPU6050_D5
#define MPU6050_INT_OPEN MPU6050_D6
#define MPU6050_INT_LEVEL MPU6050_D7
// INT_ENABLE Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DATA_RDY_EN MPU6050_D0
#define MPU6050_I2C_MST_INT_EN MPU6050_D3
#define MPU6050_FIFO_OFLOW_EN MPU6050_D4
#define MPU6050_ZMOT_EN MPU6050_D5
#define MPU6050_MOT_EN MPU6050_D6
#define MPU6050_FF_EN MPU6050_D7
// INT_STATUS Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DATA_RDY_INT MPU6050_D0
#define MPU6050_I2C_MST_INT MPU6050_D3
#define MPU6050_FIFO_OFLOW_INT MPU6050_D4
#define MPU6050_ZMOT_INT MPU6050_D5
#define MPU6050_MOT_INT MPU6050_D6
#define MPU6050_FF_INT MPU6050_D7
// MOT_DETECT_STATUS Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_MOT_ZRMOT MPU6050_D0
#define MPU6050_MOT_ZPOS MPU6050_D2
#define MPU6050_MOT_ZNEG MPU6050_D3
#define MPU6050_MOT_YPOS MPU6050_D4
#define MPU6050_MOT_YNEG MPU6050_D5
#define MPU6050_MOT_XPOS MPU6050_D6
#define MPU6050_MOT_XNEG MPU6050_D7
// IC2_MST_DELAY_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0
#define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1
#define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2
#define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3
#define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4
#define MPU6050_DELAY_ES_SHADOW MPU6050_D7
// SIGNAL_PATH_RESET Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_TEMP_RESET MPU6050_D0
#define MPU6050_ACCEL_RESET MPU6050_D1
#define MPU6050_GYRO_RESET MPU6050_D2
// MOT_DETECT_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_MOT_COUNT0 MPU6050_D0
#define MPU6050_MOT_COUNT1 MPU6050_D1
#define MPU6050_FF_COUNT0 MPU6050_D2
#define MPU6050_FF_COUNT1 MPU6050_D3
#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4
#define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5
// Combined definitions for the MOT_COUNT
#define MPU6050_MOT_COUNT_0 (0)
#define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0))
#define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1))
#define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0))
// Alternative names for the combined definitions
#define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0
// Combined definitions for the FF_COUNT
#define MPU6050_FF_COUNT_0 (0)
#define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0))
#define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1))
#define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0))
// Alternative names for the combined definitions
#define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0
// Combined definitions for the ACCEL_ON_DELAY
#define MPU6050_ACCEL_ON_DELAY_0 (0)
#define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0))
#define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1))
#define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0))
// Alternative names for the ACCEL_ON_DELAY
#define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0
#define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1
#define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2
#define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3
// USER_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_SIG_COND_RESET MPU6050_D0
#define MPU6050_I2C_MST_RESET MPU6050_D1
#define MPU6050_FIFO_RESET MPU6050_D2
#define MPU6050_I2C_IF_DIS MPU6050_D4 // must be 0 for MPU-6050
#define MPU6050_I2C_MST_EN MPU6050_D5
#define MPU6050_FIFO_EN MPU6050_D6
// PWR_MGMT_1 Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_CLKSEL0 MPU6050_D0
#define MPU6050_CLKSEL1 MPU6050_D1
#define MPU6050_CLKSEL2 MPU6050_D2
#define MPU6050_TEMP_DIS MPU6050_D3 // 1: disable temperature sensor
#define MPU6050_CYCLE MPU6050_D5 // 1: sample and sleep
#define MPU6050_SLEEP MPU6050_D6 // 1: sleep mode
#define MPU6050_DEVICE_RESET MPU6050_D7 // 1: reset to default values
// Combined definitions for the CLKSEL
#define MPU6050_CLKSEL_0 (0)
#define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1))
#define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2))
#define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1))
#define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
// Alternative names for the combined definitions
#define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0
#define MPU6050_CLKSEL_X MPU6050_CLKSEL_1
#define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2
#define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3
#define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4
#define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5
#define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6
#define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7
// PWR_MGMT_2 Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_STBY_ZG MPU6050_D0
#define MPU6050_STBY_YG MPU6050_D1
#define MPU6050_STBY_XG MPU6050_D2
#define MPU6050_STBY_ZA MPU6050_D3
#define MPU6050_STBY_YA MPU6050_D4
#define MPU6050_STBY_XA MPU6050_D5
#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6
#define MPU6050_LP_WAKE_CTRL1 MPU6050_D7
// Combined definitions for the LP_WAKE_CTRL
#define MPU6050_LP_WAKE_CTRL_0 (0)
#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0))
#define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1))
#define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0))
// Alternative names for the combined definitions
// The names uses the Wake-up Frequency.
#define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0
#define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1
#define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2
#define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3
// Default I2C address for the MPU-6050 is 0x68.
// But only if the AD0 pin is low.
// Some sensor boards have AD0 high, and the
// I2C address thus becomes 0x69.
#define MPU6050_I2C_ADDRESS 0x68
// Declaring an union for the registers and the axis values.
// The byte order does not match the byte order of
// the compiler and AVR chip.
// The AVR chip (on the Arduino board) has the Low Byte
// at the lower address.
// But the MPU-6050 has a different order: High Byte at
// lower address, so that has to be corrected.
// The register part "reg" is only used internally,
// and are swapped in code.
typedef union accel_t_gyro_union
{
struct
{
uint8_t x_accel_h;
uint8_t x_accel_l;
uint8_t y_accel_h;
uint8_t y_accel_l;
uint8_t z_accel_h;
uint8_t z_accel_l;
uint8_t t_h;
uint8_t t_l;
uint8_t x_gyro_h;
uint8_t x_gyro_l;
uint8_t y_gyro_h;
uint8_t y_gyro_l;
uint8_t z_gyro_h;
uint8_t z_gyro_l;
} reg;
struct
{
int16_t x_accel;
int16_t y_accel;
int16_t z_accel;
int16_t temperature;
int16_t x_gyro;
int16_t y_gyro;
int16_t z_gyro;
} value;
};
// Use the following global variables and access functions to help store the overall
// rotation angle of the sensor
unsigned long last_read_time;
float last_x_angle; // These are the filtered angles
float last_y_angle;
float last_z_angle;
float last_gyro_x_angle; // Store the gyro angles to compare drift
float last_gyro_y_angle;
float last_gyro_z_angle;
void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro) {
last_read_time = time;
last_x_angle = x;
last_y_angle = y;
last_z_angle = z;
last_gyro_x_angle = x_gyro;
last_gyro_y_angle = y_gyro;
last_gyro_z_angle = z_gyro;
}
inline unsigned long get_last_time() {return last_read_time;}
inline float get_last_x_angle() {return last_x_angle;}
inline float get_last_y_angle() {return last_y_angle;}
inline float get_last_z_angle() {return last_z_angle;}
inline float get_last_gyro_x_angle() {return last_gyro_x_angle;}
inline float get_last_gyro_y_angle() {return last_gyro_y_angle;}
inline float get_last_gyro_z_angle() {return last_gyro_z_angle;}
// Use the following global variables and access functions
// to calibrate the acceleration sensor
float base_x_accel;
float base_y_accel;
float base_z_accel;
float base_x_gyro;
float base_y_gyro;
float base_z_gyro;
int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable. Returns the error value
accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr;
int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro));
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l);
SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l);
SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l);
SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l);
SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l);
SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l);
SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l);
return error;
}
// The sensor should be motionless on a horizontal surface
// while calibration is happening
void calibrate_sensors() {
int num_readings = 10;
float x_accel = 0;
float y_accel = 0;
float z_accel = 0;
float x_gyro = 0;
float y_gyro = 0;
float z_gyro = 0;
accel_t_gyro_union accel_t_gyro;
//Serial.println("Starting Calibration");
// Discard the first set of values read from the IMU
read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
// Read and average the raw values from the IMU
for (int i = 0; i < num_readings; i++) {
read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
x_accel += accel_t_gyro.value.x_accel;
y_accel += accel_t_gyro.value.y_accel;
z_accel += accel_t_gyro.value.z_accel;
x_gyro += accel_t_gyro.value.x_gyro;
y_gyro += accel_t_gyro.value.y_gyro;
z_gyro += accel_t_gyro.value.z_gyro;
delay(100);
}
x_accel /= num_readings;
y_accel /= num_readings;
z_accel /= num_readings;
x_gyro /= num_readings;
y_gyro /= num_readings;
z_gyro /= num_readings;
// Store the raw calibration values globally
base_x_accel = x_accel;
base_y_accel = y_accel;
base_z_accel = z_accel;
base_x_gyro = x_gyro;
base_y_gyro = y_gyro;
base_z_gyro = z_gyro;
//Serial.println("Finishing Calibration");
}
// --------------------------------------------------------
// MPU6050_read
//
// This is a common function to read multiple bytes
// from an I2C device.
//
// It uses the boolean parameter for Wire.endTransMission()
// to be able to hold or release the I2C-bus.
// This is implemented in Arduino 1.0.1.
//
// Only this function is used to read.
// There is no function for a single byte.
//
int MPU6050_read(int start, uint8_t *buffer, int size)
{
int i, n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1)
return (-10);
n = Wire.endTransmission(false); // hold the I2C-bus
if (n != 0)
return (n);
// Third parameter is true: relase I2C-bus after data is read.
Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
i = 0;
while(Wire.available() && iif ( i != size)
return (-11);
return (0); // return : no error
}
// --------------------------------------------------------
// MPU6050_write
//
// This is a common function to write multiple bytes to an I2C device.
//
// If only a single register is written,
// use the function MPU_6050_write_reg().
//
// Parameters:
// start : Start address, use a define for the register
// pData : A pointer to the data to write.
// size : The number of bytes to write.
//
// If only a single register is written, a pointer
// to the data has to be used, and the size is
// a single byte:
// int data = 0; // the data to write
// MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
//
int MPU6050_write(int start, const uint8_t *pData, int size)
{
int n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start); // write the start address
if (n != 1)
return (-20);
n = Wire.write(pData, size); // write data bytes
if (n != size)
return (-21);
error = Wire.endTransmission(true); // release the I2C-bus
if (error != 0)
return (error);
return (0); // return : no error
}
// --------------------------------------------------------
// MPU6050_write_reg
//
// An extra function to write a single register.
// It is just a wrapper around the MPU_6050_write()
// function, and it is only a convenient function
// to make it easier to write a single register.
//
int MPU6050_write_reg(int reg, uint8_t data)
{
int error;
error = MPU6050_write(reg, &data, 1);
return (error);
}
// my setup mpu-6050
void init_mpu6050(void) {
int error;
uint8_t c;
// Initialize the 'Wire' class for the I2C-bus.
Wire.begin();
// default at power-up:
// Gyro at 250 degrees second
// Acceleration at 2g
// Clock source at internal 8MHz
// The device is in sleep mode.
//
error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
/*
Serial.print(F("WHO_AM_I : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
*/
// According to the datasheet, the 'sleep' bit
// should read a '1'. But I read a '0'.
// That bit has to be cleared, since the sensor
// is in sleep mode at power-up. Even if the
// bit reads '0'.
error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
/*
Serial.print(F("PWR_MGMT_2 : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
*/
// Clear the 'sleep' bit to start the sensor.
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
//Initialize the angles
calibrate_sensors();
set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
}
//
float get_angle(unsigned long t_now) {
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
/*
Serial.println(F(""));
Serial.println(F("MPU-6050"));
*/
// Read the raw values.
error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
// Get the time of reading for rotation computations
// unsigned long t_now = millis();
/*
Serial.print(F("Read accel, temp and gyro, error = "));
Serial.println(error,DEC);
// Print the raw acceleration values
Serial.print(F("accel x,y,z: "));
Serial.print(accel_t_gyro.value.x_accel, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.y_accel, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.z_accel, DEC);
Serial.println(F(""));
*/
// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet:
// 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 - (340 * 35) = -12412
/*
Serial.print(F("temperature: "));
dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
Serial.print(dT, 3);
Serial.print(F(" degrees Celsius"));
Serial.println(F(""));
// Print the raw gyro values.
Serial.print(F("raw gyro x,y,z : "));
Serial.print(accel_t_gyro.value.x_gyro, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.y_gyro, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.z_gyro, DEC);
Serial.print(F(", "));
Serial.println(F(""));
*/
// Convert gyro values to degrees/sec
float FS_SEL = 131;
/*
float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL;
float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL;
float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL;
*/
float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL;
float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL;
float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL;
// Get raw acceleration values
//float G_CONVERT = 16384;
float accel_x = accel_t_gyro.value.x_accel;
float accel_y = accel_t_gyro.value.y_accel;
float accel_z = accel_t_gyro.value.z_accel;
// Get angle values from accelerometer
float RADIANS_TO_DEGREES = 180/3.14159;
//float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
float accel_angle_z = atan(sqrt(pow(accel_x,2) + pow(accel_y,2))/accel_z)*RADIANS_TO_DEGREES;;
//float accel_angle_z = 0;
// Compute the (filtered) gyro angles
float dt =(t_now - get_last_time())/1000.0;
float gyro_angle_x = gyro_x*dt + get_last_x_angle();
float gyro_angle_y = gyro_y*dt + get_last_y_angle();
float gyro_angle_z = gyro_z*dt + get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle - choice of alpha is
// estimated now. Alpha depends on the sampling rate...
float alpha = 0.96;
float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle
// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
// Send the data to the serial port
// uncomment a different version if the receiving app is different.
//
// if (angle_x > 90) {
// angle_x = 90;
// }
// if (angle_x < -90) {
// angle_x = -90;
// }
return -angle_x;
}
/*****************************************************************************************************************/
/**
* myhardware
*/
const int RELAY1_PIN = 11;
const int RELAY2_PIN = 12;
const int KEY_DOWN_TIMES = 100;
const int KEY_UP_TIMES = 500;
const int LED_FREQUENCY = 100;
const int KEY1_PIN = 5;
const int KEY2_PIN = 7;
const int KEY1_VCC_PIN = 4;
const int KEY1_GND_PIN = 3;
const int KEY2_VCC_PIN = 6;
const int KEY2_GND_PIN = 8;
boolean key1_down = false;
boolean key2_down = false;
const int RED_LED_PIN = A2;
const int RED_LED_GND_PIN = A3;
const int GREEN_LED_PIN = A0;
const int GREEN_LED_GND_PIN= A1;
enum { O_STATE = '0',
A_STATE = 'A',
B_STATE = 'B'};
char device_state = O_STATE;
int key_down_counter = 0;
int key_up_counter = 0;
char automatic = true;
int first = 1;
int offset_angle = 0;
int LED_TIMES = 0;
void setup() {
// put your setup code here, to run once:
init_key();
init_relay();
init_led();
init_mpu6050();
Serial.begin(115200);
byte high = EEPROM.read(0);
byte low = EEPROM.read(1);
offset_angle = (high << 8) + low;
// for (int i = 0; i < 1023; i++) {
// EEPROM.write(i, 0);
}
void loop() {
float angle;
// put your main code here, to run repeatedly:
update_key();
//
unsigned long t_now = millis();
if (automatic == true) {
if (LED_TIMES >= (LED_FREQUENCY/ 2)) {
digitalWrite(GREEN_LED_PIN, HIGH);
digitalWrite(RED_LED_PIN, LOW);
}
else {
digitalWrite(GREEN_LED_PIN, LOW);
digitalWrite(RED_LED_PIN, LOW);
}
LED_TIMES += 1;
if (LED_TIMES >= LED_FREQUENCY) {
LED_TIMES = 0;
}
if ((key1_down ==true) && (key2_down == true)) {
key_down_counter += 1;
}
else {
key_down_counter = 0;
}
if (key_down_counter >= KEY_DOWN_TIMES) {
key_down_counter = KEY_DOWN_TIMES;
automatic = false;
first = 1;
key_up_counter = 0;
LED_TIMES = 0;
}
angle = get_angle(t_now) - ((offset_angle / 100.0) - 90);
if (change_device_state(angle) != 0) {
Serial.print(".........\r\n");
}
action_by_state(device_state);
Serial.print("angle:");
Serial.print(angle);
Serial.print(" ");
Serial.print("offset_angle:");
Serial.print(((offset_angle / 100.0) - 90));
Serial.print(" ");
Serial.print("device_state:");
Serial.print(device_state);
Serial.print(" ");
Serial.print("key_down_counter:");
Serial.print(key_down_counter);
Serial.print("\r\n");
}
else {
red_led_on();
green_led_off();
if ((key1_down == false) && (key2_down == false)) {
first = 0;
relay1_off();
relay2_off();
key_up_counter += 1;
offset_angle = (get_angle(t_now) + 90) * 100;
}
else {
if ((key1_down == true) && (key2_down == false)) {
if (first == 0) {
relay1_on();
relay2_off();
}
}
if ((key1_down == false) && (key2_down == true)) {
if (first == 0) {
relay1_off();
relay2_on();
}
}
key_up_counter = 0;
}
if (key_up_counter >= KEY_UP_TIMES) {
key_up_counter = KEY_UP_TIMES;
automatic = true;
key_down_counter = 0;
EEPROM.write(0, highByte(offset_angle));
EEPROM.write(1, lowByte(offset_angle));
}
Serial.print("angle:");
Serial.print(get_angle(t_now));
Serial.print(" ");
Serial.print("offset_angle:");
Serial.print(((offset_angle / 100.0) - 90));
Serial.print("\r\n");
}
delay(10);
}
// key_Vcc
void init_key() {
// key
pinMode(KEY1_PIN, INPUT);
pinMode(KEY2_PIN, INPUT);
//
pinMode(KEY1_VCC_PIN, OUTPUT);
pinMode(KEY2_VCC_PIN, OUTPUT);
// key_Gnd
pinMode(KEY2_GND_PIN, OUTPUT);
pinMode(KEY1_GND_PIN, OUTPUT);
// key_Vcc and key_Gnd
digitalWrite(KEY1_VCC_PIN, HIGH);
digitalWrite(KEY2_VCC_PIN, HIGH);
digitalWrite(KEY2_GND_PIN, LOW);
digitalWrite(KEY1_GND_PIN, LOW);
}
// Getting stats of key
void update_key() {
key1_down = digitalRead(KEY1_PIN) == HIGH? true: false;
key2_down = digitalRead(KEY2_PIN) == HIGH? true: false;
}
//
void init_relay(void) {
// Relay
pinMode(RELAY1_PIN, OUTPUT);
pinMode(RELAY2_PIN, OUTPUT);
//
digitalWrite(RELAY1_PIN, HIGH);
digitalWrite(RELAY2_PIN, HIGH);
}
//
void relay1_on(void) {
digitalWrite(RELAY1_PIN, LOW);
}
//
void relay1_off(void) {
digitalWrite(RELAY1_PIN, HIGH);
}
//
void relay2_on(void) {
digitalWrite(RELAY2_PIN, LOW);
}
//
void relay2_off(void) {
digitalWrite(RELAY2_PIN, HIGH);
}
//
void init_led(void) {
// LED
pinMode(RED_LED_PIN, OUTPUT);
pinMode(GREEN_LED_PIN, OUTPUT);
pinMode(RED_LED_GND_PIN, OUTPUT);
pinMode(GREEN_LED_GND_PIN, OUTPUT);
// LED GND
digitalWrite(RED_LED_GND_PIN, LOW);
digitalWrite(GREEN_LED_GND_PIN, LOW);
}
//
void red_led_on(void) {
digitalWrite(RED_LED_PIN, HIGH);
}
//
void red_led_off(void) {
digitalWrite(RED_LED_PIN, LOW);
}
//
void green_led_on(void) {
digitalWrite(GREEN_LED_PIN, HIGH);
}
//
void green_led_off(void) {
digitalWrite(GREEN_LED_PIN, LOW);
}
//
void control_relay(void) {
if (key1_down == true) {
relay1_on();
red_led_on();
}
else {
relay1_off();
red_led_off();
}
if (key2_down == true) {
relay2_on();
green_led_on();
}
else {
relay2_off();
green_led_off();
}
}
//
int change_device_state(float angle) {
int err = 0;
if (angle >= 5) {
device_state = A_STATE;
}
else if (angle <= -5) {
device_state = B_STATE;
}
else if ((angle >= -0.5) && (angle <= 0.5)) {
device_state = O_STATE;
}
else if ((angle > 0.5) && (angle < 5)) {
if (device_state == B_STATE) {
err = 1;
device_state = O_STATE;
}
}
else {
if (device_state == A_STATE) {
err = 1;
device_state = O_STATE;
}
}
return err;
}
//
void action_by_state(char state) {
if (state == A_STATE) {
relay1_on();
relay2_off();
}
else if (state == B_STATE) {
relay1_off();
relay2_on();
}
else {
relay1_off();
relay2_off();
}
}