testing gyro and compass module on i2c

SE/stuff/P5_Automation_can-dev-res-working-1930/___SUBMODULES___/gyro_and_compass_test/ctrl.h

@@ -0,0 +1 @@
+

SE/stuff/P5_Automation_can-dev-res-working-1930/___SUBMODULES___/gyro_and_compass_test/gyro_and_compass_test.ino

@@ -0,0 +1,101 @@
+ #include "I2Cdev.h"
+#include "MPU6050.h"
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+    #include "Wire.h"
+#endif
+  #include "struct.h"
+
+#include <QMC5883LCompass.h>
+QMC5883LCompass compass;
+
+
+const int MPU=0x68; 
+int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
+
+MPU6050 accelgyro(MPU);
+
+  int16_t ax, ay, az;
+  int16_t gx, gy, gz;
+#define OUTPUT_READABLE_ACCELGYRO
+//#define OUTPUT_BINARY_ACCELGYRO
+
+
+#define LED_PIN 13
+bool blinkState = false;
+
+ 
+void setup()
+{
+  
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+        Wire.begin();
+    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+        Fastwire::setup(400, true);
+    #endif
+    Serial.begin(115200);
+     while (!Serial); // Waiting for Serial Monitor
+    Serial.println("\nCompass ang gyro  test  ");      
+    #include "scan.h"
+
+    Serial.println("compass.init()");
+    compass.init();
+    Serial.println("accelgyro.initialize()");
+    accelgyro.initialize();
+    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+     accelgyro.setMotionDetectionDuration(80);
+
+    
+    pinMode(LED_PIN, OUTPUT);
+
+     
+
+ // setZeroMotionDetectionThreshold(154);
+}
+ 
+
+void loop()
+{
+   int x, y, z;
+Serial.println();
+  // Read compass values
+  compass.read();
+
+  x = compass.getX();
+  y = compass.getY();
+  z = compass.getZ();
+
+  Serial.print("Comp X: ");
+  Serial.print(x);
+  Serial.print("   Y: ");
+  Serial.print(y);
+  Serial.print("   Z: ");
+  Serial.print(z);
+
+    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+   // these methods (and a few others) are also available
+    //accelgyro.getAcceleration(&ax, &ay, &az);
+    //accelgyro.getRotation(&gx, &gy, &gz);
+#ifdef OUTPUT_READABLE_ACCELGYRO
+        // display tab-separated accel/gyro x/y/z values
+        Serial.print("  a/g:\t");
+        Serial.print(ax); Serial.print("\t");
+        Serial.print(ay); Serial.print("\t");
+        Serial.print(az); Serial.print("\t");
+        Serial.print(gx); Serial.print("\t");
+        Serial.print(gy); Serial.print("\t");
+        Serial.print(gz);
+    #endif
+
+    #ifdef OUTPUT_BINARY_ACCELGYRO
+        Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
+        Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
+        Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
+        Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
+        Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
+        Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
+    #endif
+
+  delay(1500);
+
+}

SE/stuff/P5_Automation_can-dev-res-working-1930/___SUBMODULES___/gyro_and_compass_test/scan.h

@@ -0,0 +1,41 @@
+
+{
+  byte error, address; //variable for error and I2C address
+  int nDevices;
+
+  Serial.println("Scanning.1..");
+
+  nDevices = 0;
+  for (address = 1; address < 127; address++ )
+  {
+    // The i2c_scanner uses the return value of
+    // the Write.endTransmisstion to see if
+    // a device did acknowledge to the address.
+    Wire.beginTransmission(address);
+    error = Wire.endTransmission();
+  //Serial.print(address);
+
+    if (error == 0)
+    {
+      Serial.print("I2C device found at address 0x");
+      if (address < 16)
+        Serial.print("0");
+      Serial.print(address, HEX);
+      Serial.println("  !");
+      nDevices++;
+    }
+    else if (error == 4)
+    {
+      Serial.print("Unknown error at address 0x");
+      if (address < 16)
+        Serial.print("0");
+      Serial.println(address, HEX);
+    }
+  }
+  if (nDevices == 0)
+    Serial.println("No I2C devices found\n");
+  else
+    Serial.println("done\n");
+
+ // delay(5000); // wait 5 seconds for the next I2C scan
+}

SE/stuff/P5_Automation_can-dev-res-working-1930/___SUBMODULES___/gyro_and_compass_test/struct.h

@@ -0,0 +1,39 @@
+
+typedef   byte step_byte_T ;
+typedef   byte step_curr_T ;
+
+ struct step_seq_S {
+  step_byte_T step_byte_A ;
+};
+
+  typedef struct step_seq_S step_seq_T ;
+
+   step_curr_T step_currX_A = 1;
+   step_curr_T step_currY_A = 1;
+   step_seq_T step_seqX_A[8]  ;
+   step_seq_T step_seqY_A[8]  ;
+//  step_seq_A[0] = { 1 };
+
+  step_seq_T step_seq_init(step_seq_T* step_seqX_A, step_seq_T* step_seqY_A) {
+         step_seqX_A[1].step_byte_A = B00000001  ;
+         step_seqX_A[2].step_byte_A = B00000011 ;
+         step_seqX_A[3].step_byte_A = B00000010 ;
+         step_seqX_A[4].step_byte_A = B00000110 ;
+         step_seqX_A[5].step_byte_A = B00000100 ;
+         step_seqX_A[6].step_byte_A = B00001100 ;
+         step_seqX_A[7].step_byte_A = B00001000 ;
+         step_seqX_A[8].step_byte_A = B00001001 ;
+
+         step_seqY_A[1].step_byte_A = B00010000 ;
+         step_seqY_A[2].step_byte_A = B00110000 ;
+         step_seqY_A[3].step_byte_A = B00100000 ;
+         step_seqY_A[4].step_byte_A = B01100000 ;
+         step_seqY_A[5].step_byte_A = B01000000 ;
+         step_seqY_A[6].step_byte_A = B11000000 ;
+         step_seqY_A[7].step_byte_A = B10000000 ;
+         step_seqY_A[8].step_byte_A = B10010000 ;
+         
+  }
+
+
+