Update 05-IMUuLCD.ino

diff --git a/05-IMUuLCD/05-IMUuLCD.ino b/05-IMUuLCD/05-IMUuLCD.ino
index 74f966a4e2fe85eb6097ef342c3d3f2633e66ad8..c4b51e48758e4b42af9d424a187370cbde9a0fd9 100644 (file)
--- a/05-IMUuLCD/05-IMUuLCD.ino
+++ b/05-IMUuLCD/05-IMUuLCD.ino
@@ -1,6 +1,215 @@
+#include "ICM_20948.h"
+#include "Goldelox_Serial_4DLib.h"
+#include "Goldelox_Const4D.h"
+#define DisplaySerial Serial1
+
+//imu pin stuff
+#define SERIAL_PORT Serial
+#define WIRE_PORT Wire
+#define AD0_VAL 1
+#define SDApin 21
+#define SCLpin 22
+ICM_20948_I2C myICM;
+
+//lcd pin stuff
+Goldelox_Serial_4DLib Display(&DisplaySerial);
+#define resetLine 20
+#define LCDTx 4
+#define LCDRx 5
+word CircleX = 64;
+word CircleY = 64;
+int rad = 5;
+
+int velocity = 1;
+ICM_20948_I2C *point;
+void printScaledAGMT(ICM_20948_I2C *sensor);
+void speed(int data); 
+void printFormattedFloat(float val, uint8_t leading, uint8_t decimals);
+//setting up color change function beloow
+void colorChange();
+word colors[] = {BLUE, RED, GREEN, YELLOW, PURPLE, ORANGE};
+word currColor;
+int count;
 
 void setup() {
+  // imu setup
+  SERIAL_PORT.begin(115200);
+  while (!SERIAL_PORT) {
+  };
+  WIRE_PORT.begin(SDApin, SCLpin);
+  WIRE_PORT.setClock(400000);
+  bool initialized = false;
+  while(!initialized) {
+    myICM.begin(WIRE_PORT, AD0_VAL);
+    SERIAL_PORT.print(F("Initialization of the sensor returned: "));
+    SERIAL_PORT.println(myICM.statusString());
+    if (myICM.status != ICM_20948_Stat_Ok) {
+      SERIAL_PORT.println("Trying again...");
+      delay(500);
+    }
+    else {
+      initialized = true;
+    }
+  }
+
+  //lcd setup
+  pinMode(resetLine, OUTPUT);
+  digitalWrite(resetLine, 0);
+  delay(100);
+  digitalWrite(resetLine, 1); 
+  delay(5000);
+
+  DisplaySerial.begin(9600, SERIAL_8N1, LCDRx, LCDTx); 
+  Display.TimeLimit4D = 5000;
+  Display.gfx_Cls();
+  Display.gfx_CircleFilled(CircleX, CircleY, rad, BLUE);
+  point = &myICM;
+  count = 0;
+  currColor = colors[0];
 }
 
 void loop() {
+  // imu setup
+  if (myICM.dataReady()) {
+    myICM.getAGMT();
+    printScaledAGMT(&myICM);
+    delay(30);
+  }
+  else {
+    SERIAL_PORT.println("Waiting for data");
+    delay(500);
+  }
+
+  // erase old ball
+  Display.gfx_CircleFilled(CircleX, CircleY, rad+2, BLACK);
+
+  // movement in X
+  if (point->accX() < 0) { // tilted left
+    speed(point->accX());
+    CircleX -= velocity;
+  }
+  else if (point->accX() > 0) { // tilted right
+    speed(point->accX());
+    CircleX += velocity;
+  }
+
+  // movement in Y
+  if (point->accY() < 0) { // tilted down
+    speed(point->accY());
+    CircleY += velocity;
+  }
+  else if (point->accY() > 0) { // tilted up
+    speed(point->accY());
+    CircleY -= velocity;
+  }
+  //keeping it in bounds
+  bool bounced = false;
+  if (CircleX <= rad) {
+    CircleX = rad;
+    bounced = true;
+  }
+  if (CircleX >= 127 - rad) {
+    CircleX = 127 - rad;
+    bounced = true;
+  }
+  if (CircleY <= rad) {
+    CircleY = rad;
+    bounced = true;
+  }
+  if (CircleY >= 127 - rad) {
+    CircleY = 127 - rad;
+    bounced = true;
+  }
+
+  // change color on bounce
+  if (bounced) {
+    colorChange();
+  }
+
+  // draw new ball
+  Display.gfx_CircleFilled(CircleX, CircleY, rad, currColor);
+
+  delay(50);
+}
+
+void speed(int data) { 
+    int last = velocity;
+    velocity = abs(data)/100;
+    if (velocity == 0){
+      velocity = last;
+    }
+  }
+void colorChange() { //omg this one changes the color of the ball
+  count++;
+  if (count == 6) {
+    count = 0;
+  }
+  currColor = colors[count];
+}
+//helper functions from example
+void printFormattedFloat(float val, uint8_t leading, uint8_t decimals) //this function is to watch on serial monitor
+{
+  float aval = abs(val);
+  if (val < 0)
+  {
+    SERIAL_PORT.print("-");
+  }
+  else
+  {
+    SERIAL_PORT.print(" ");
+  }
+  for (uint8_t indi = 0; indi < leading; indi++)
+  {
+    uint32_t tenpow = 0;
+    if (indi < (leading - 1))
+    {
+      tenpow = 1;
+    }
+    for (uint8_t c = 0; c < (leading - 1 - indi); c++)
+    {
+      tenpow *= 10;
+    }
+    if (aval < tenpow)
+    {
+      SERIAL_PORT.print("0");
+    }
+    else
+    {
+      break;
+    }
+  }
+  if (val < 0)
+  {
+    SERIAL_PORT.print(-val, decimals);
+  }
+  else
+  {
+    SERIAL_PORT.print(val, decimals);
+  }
+}
+
+void printScaledAGMT(ICM_20948_I2C *sensor) //also to watch on serial monitor
+{
+  SERIAL_PORT.print("Scaled. Acc (mg) [ ");
+  printFormattedFloat(sensor->accX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->accY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->accZ(), 5, 2);
+  SERIAL_PORT.print(" ], Gyr (DPS) [ ");
+  printFormattedFloat(sensor->gyrX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->gyrY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->gyrZ(), 5, 2);
+  SERIAL_PORT.print(" ], Mag (uT) [ ");
+  printFormattedFloat(sensor->magX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->magY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->magZ(), 5, 2);
+  SERIAL_PORT.print(" ], Tmp (C) [ ");
+  printFormattedFloat(sensor->temp(), 5, 2);
+  SERIAL_PORT.print(" ]");
+  SERIAL_PORT.println();
 }