質問編集履歴

こ

2021/01/29 07:44

投稿

_mini

スコア15

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -8,6 +8,286 @@
+arudinoのコード
+```ここに言語を入力
+#include <Wire.h>
+#include <MadgwickAHRS.h>
+Madgwick MadgwickFilter;
+#define MPU6050_PWR_MGMT_1   0x6B
+#define MPU_ADDRESS  0x68
+void setup() {
+  Wire.begin();
+  Serial.begin(115200); //115200bps
+  Wire.beginTransmission(MPU_ADDRESS);
+  Wire.write(MPU6050_PWR_MGMT_1);  //MPU6050_PWR_MGMT_1レジスタの設定
+  Wire.write(0x00);
+  Wire.endTransmission();
+  MadgwickFilter.begin(100); //100Hz
+}
+void loop() {
+  Wire.beginTransmission(0x68);
+  Wire.write(0x3B);
+  Wire.endTransmission(false);
+  Wire.requestFrom(0x68, 14, true);
+  while (Wire.available() < 14);
+  int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
+  axRaw = Wire.read() << 8 | Wire.read();
+  ayRaw = Wire.read() << 8 | Wire.read();
+  azRaw = Wire.read() << 8 | Wire.read();
+  Temperature = Wire.read() << 8 | Wire.read();
+  gxRaw = Wire.read() << 8 | Wire.read();
+  gyRaw = Wire.read() << 8 | Wire.read();
+  gzRaw = Wire.read() << 8 | Wire.read();
+  // 加速度値を分解能で割って加速度(G)に変換する
+  float acc_x = axRaw / 16384.0;  //FS_SEL_0 16,384 LSB / g
+  float acc_y = ayRaw / 16384.0;
+  float acc_z = azRaw / 16384.0;
+  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
+  float gyro_x = gxRaw / 131.0;  // (度/s)
+  float gyro_y = gyRaw / 131.0;
+  float gyro_z = gzRaw / 131.0;
+  //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
+  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
+  //PRYの計算結果を取得する
+  float roll_1  = MadgwickFilter.getRoll();
+  float pitch_1 = MadgwickFilter.getPitch();
+  float yaw   = MadgwickFilter.getYaw();
+  int roll_int,pitch_int,bitshift,i=1;
+  roll_int = int(roll_1) << 8;
+  pitch_int = int(pitch_1 * -1);
+  bitshift = roll_int + pitch_int;
+//  Serial.print("roll:");Serial.print(roll);  Serial.print(",");
+//  Serial.print("pitch:"); Serial.print(pitch);  Serial.println(",");
+  Serial.println(bitshift);
+```
+pythonのコード
+今回の質問と直接関係ない部分は省かせていただいております。
+```ここに言語を入力
+# coding: UTF-8
+# ジョイスティックWHILL操作システム
+import time
+from pygame.time import wait
+import serial
+from whill import ComWHILL
+import pygame
+from pygame.locals import *
+import sys
+import copy
+import math
+# 定数定義
+SCREEN_WIDTH = 800
+SCREEN_HEIGHT = 600
+LEVELSTEPNUM = 10
+ser = serial.Serial("COM7", 115200)  # Arduinoが接続されているCOMポートを指定
+def main():
+    global startTime
+    pygame.init()  # 初期化
+    screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))  # 画面を作る
+    pygame.display.set_caption("whill controller")
+     font = pygame.font.Font(None, 30)
+    clock = pygame.time.Clock()
+    FPS = 120
+    # メイン関数
+    while True:
+        # デバイスの入力
+        getDeviceData()
+        # キー入力で終了
+        for event in pygame.event.get():
+            if event.type == QUIT:
+                pygame.quit()
+                sys.exit()
+            if event.type == KEYDOWN:
+                if event.key == K_ESCAPE:
+                    pygame.quit()
+                    sys.exit()
+            pygame.display.update()
+        clock.tick(FPS)
+# 傾き判別
+def getDeviceData():
+    Speed, RotationSpeed = 0, 0
+    getData1 = str(ser.readline().rstrip().decode(
+        encoding='utf-8', errors='ignore'))
+    data = int(getData1)
+    Speed = (data & 65280) >> 8
+    RotationSpeed = data & 255
+    if Speed > 127:
+      　Speed -= 255
+    if RotationSpeed > 127:
+        RotationSpeed -= 255
+    print(Speed, RotationSpeed)
+if __name__ == "__main__":
+    main()
+```
 ![イメージ説明](5543b7a7cbe70581f55db6c5698aa3a2.png)