did a lot of thing lately

1 files changed, 0 insertions, 116 deletions

diff --git a/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/IMU_comp.ino b/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/IMU_comp.ino
deleted file mode 100644
index c4578cb..0000000
--- a/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/IMU_comp.ino
+++ /dev/null
@@ -1,116 +0,0 @@
-/*
-This sketch combines magnetometer readings from an LIS3MDL and accelerometer
-readings from an LSM6 to calculate a tilt-compensated magnetic heading. It
-requires Pololu's LSM6 Arduino library to be installed:
-
-https://github.com/pololu/lsm6-arduino
-
-This program can be used with a board that includes both sensors, like the
-Pololu MinIMU-9 v5 and AltIMU-10 v5, or with separate carrier boards for the two
-sensors, both connected to the same I2C bus. If you are using separate boards,
-make sure the axes are oriented the same way on both (i.e. the X, Y, and Z axes
-of both boards should point in the same direction, and the surfaces of the
-boards should be as close to parallel as possible).
-*/
-
-#include <Wire.h>
-#include <LIS3MDL.h>
-#include <LSM6.h>
-
-LIS3MDL mag;
-LSM6 imu;
-int front = 0;
-
-/*
-Calibration values; the default values of +/-32767 for each axis
-lead to an assumed magnetometer bias of 0. Use the Calibrate example
-program to determine appropriate values for your particular unit.
-*/
-LIS3MDL::vector<int16_t> m_min = {+942, +3436, -3323};
-LIS3MDL::vector<int16_t> m_max = {+1053, +3559, -3142};
-
-void setup()
-{
-  Serial.begin(9600);
-  Wire.begin();
-
-  if (!mag.init())
-  {
-    Serial.println("Failed to detect and initialize LIS3MDL magnetometer!");
-    while (1);
-  }
-  mag.enableDefault();
-
-  if (!imu.init())
-  {
-    Serial.println("Failed to detect and initialize LSM6 IMU!");
-    while (1);
-  }
-  imu.enableDefault();
-
-  //get start heading
-  mag.read();
-  imu.read();
-  float heading = computeHeading();
-  front = heading;
-  Serial.println(heading);
-
-}
-
-void loop()
-{
-  mag.read();
-  imu.read();
-
-
-  float heading = computeHeading();
-  int offset = front - heading;
-  if(offset<0){
-    offset=360+offset;
-  }
-  
-  Serial.print("heading: ");
-  Serial.print(heading);
-  Serial.print(", goal location: ");
-  Serial.print(front);
-  Serial.print(", offset: ");
-  
-  Serial.println(offset);
-
-  delay(100);
-}
-
-template <typename T> float computeHeading(LIS3MDL::vector<T> from)
-{
-  LIS3MDL::vector<int32_t> temp_m = {mag.m.x, mag.m.y, mag.m.z};
-
-  // copy acceleration readings from LSM6::vector into an LIS3MDL::vector
-  LIS3MDL::vector<int16_t> a = {imu.a.x, imu.a.y, imu.a.z};
-
-  // subtract offset (average of min and max) from magnetometer readings
-  temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
-  temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
-  temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
-
-  // compute E and N
-  LIS3MDL::vector<float> E;
-  LIS3MDL::vector<float> N;
-  LIS3MDL::vector_cross(&temp_m, &a, &E);
-  LIS3MDL::vector_normalize(&E);
-  LIS3MDL::vector_cross(&a, &E, &N);
-  LIS3MDL::vector_normalize(&N);
-
-  // compute heading
-  float heading = atan2(LIS3MDL::vector_dot(&E, &from), LIS3MDL::vector_dot(&N, &from)) * 180 / PI;
-  if (heading < 0) heading += 360;
-  return heading;
-}
-
-/*
-Returns the angular difference in the horizontal plane between a
-default vector (the +X axis) and north, in degrees.
-*/
-float computeHeading()
-{
-  return computeHeading((LIS3MDL::vector<int>){1, 0, 0});
-}