uint8_t buff[6];
short X, Y, Z;
int ix, iy;
float xf, yf, zf, l, q;
while (1)
{
X = Y = Z = 0;
memset(buff, 0x00 , sizeof(buff));
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[0], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[0], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[1], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[1], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[2], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[2], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[3], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[3], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[4], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[4], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[5], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[5], 1, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x32, &acc_data[6], 1, 1000);
HAL_I2C_Master_Receive(&hi2c1, 0x33, &buff[6], 1, 1000);
X = ((short)buff[1] << 8) | (short)buff[0];
X = X >> 4;
xf = X;
Y = ((short)buff[3] << 8) | (short)buff[2];
Y = Y >> 4;
yf = Y;
Z = ((short)buff[5] << 8) | (short)buff[4];
Z = Z >> 4;
zf = Z;
zf = zf / 1000;
zf = acosf(zf);
if(X < 0) ix = 1;
else ix = 0;
xf = fabsf((float)X);
if(Y < 0) iy = 1;
else iy = 0;
yf = fabsf((float)Y);
l = sqrtf(powf(xf, 2.0) + powf(yf, 2.0));
q = xf / l;
if(zf < DEG_05) { // 5.0DEG
HAL_GPIO_WritePin(GPIOE,
GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 |
GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15,
GPIO_PIN_SET);
}
else if(zf < DEG_10) { // 10.0DEG
if(ix) {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_RESET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_15 | GPIO_PIN_8, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_RESET);
}
}
}
else {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_RESET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_RESET);
}
}
}
}
else if(zf < DEG_30) { // 30.0DEG
if(ix) {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_12 | GPIO_PIN_14, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_11 | GPIO_PIN_13, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_RESET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_14 | GPIO_PIN_8, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_12 | GPIO_PIN_14, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_13 | GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_RESET);
}
}
}
else {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_8 | GPIO_PIN_10, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_9 | GPIO_PIN_11, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_8, GPIO_PIN_RESET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15 | GPIO_PIN_14 | GPIO_PIN_8, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_RESET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_8 | GPIO_PIN_10, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_RESET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_15 | GPIO_PIN_9, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_RESET);
}
}
}
}
else if(zf < DEG_45) { // 45.0DEG
HAL_GPIO_WritePin(GPIOE,
GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 |
GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15,
GPIO_PIN_RESET);
if(ix) {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_SET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_SET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_SET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_SET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14, GPIO_PIN_SET);
}
}
}
else {
if(iy) {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_SET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_SET);
}
}
else {
if(q < COS_1) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_SET);
}
else if (q > COS_2) {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_SET);
}
else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET);
}
}
}
}
else {
HAL_GPIO_WritePin(GPIOE,
GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11 |
GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15,
GPIO_PIN_RESET);
}
}
|
コメント
コメントを投稿