Create 5.5-ReplaceBallwText

diff --git a/5.5-ReplaceBallwText b/5.5-ReplaceBallwText
new file mode 100644 (file)
index 0000000..c45b55d
--- /dev/null
+++ b/5.5-ReplaceBallwText
@@ -0,0 +1,141 @@
+#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;
+
+String dir = "";
+
+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);
+  delay(500);
+  Display.gfx_CircleFilled(CircleX, CircleY, rad, GREEN); //green light lol
+  point = &myICM;
+  count = 0;
+  currColor = colors[0];
+}
+
+void loop() {
+  if (myICM.dataReady()) {
+    myICM.getAGMT();
+    delay(30);
+  } else {
+    SERIAL_PORT.println("Waiting for data");
+    delay(500);
+  }
+
+  // erase old text 
+  Display.gfx_RectangleFilled(CircleX, CircleY, CircleX + 50, CircleY + 10, BLACK);
+
+  dir = "";
+
+  if (point->accX() < -200) {         // tilt left
+    speed(point->accX());
+    CircleX -= velocity;
+    dir += "LEFT";
+  }
+  else if (point->accX() > 200) {     // tilt right
+    speed(point->accX());
+    CircleX += velocity;
+    dir += "RIGHT";
+  }
+
+  // determine vertical movement
+  if (point->accY() < -200) {         // tilt down
+    speed(point->accY());
+    CircleY += velocity;
+    if (dir.length() > 0) dir = "DOWN-" + dir;  
+    else dir += "DOWN";
+  }
+  else if (point->accY() > 200) {     // tilt up
+    speed(point->accY());
+    CircleY -= velocity;
+    if (dir.length() > 0) dir = "UP-" + dir;
+    else dir += "UP";
+  }
+
+  // keep text inside bounds
+  CircleX = constrain(CircleX, 0, 120);
+  CircleY = constrain(CircleY, 0, 120);
+
+  // draw new text
+  Display.gfx_MoveTo(CircleX, CircleY);
+  Display.txt_FGcolour(WHITE);
+
+  char buffer[20];                  // convert String to char* for Goldelox
+  dir.toCharArray(buffer, sizeof(buffer));
+  Display.putstr(buffer);
+  delay(200);
+}
+
+void speed(int data) {
+    int last = velocity;
+    velocity = abs(data)/100;
+    if (velocity == 0){
+      velocity = last;
+    }
+  }
+void colorChange() {
+  count++;
+  if (count == 6) {
+    count = 0;
+  }
+  currColor = colors[count];
+}