update repo structure

update poster
add code flowchart

19 files changed, 1486 insertions, 0 deletions

diff --git a/debug/IMU_comp/IMU_comp.ino b/debug/IMU_comp/IMU_comp.ino
new file mode 100644
index 0000000..40d99b6
--- /dev/null
+++ b/debug/IMU_comp/IMU_comp.ino
@@ -0,0 +1,32 @@
+//https://www.pololu.com/product/2862
+#include <Wire.h>
+#include <LIS3MDL.h>
+
+LIS3MDL mag;
+
+char report[80];
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!mag.init())
+  {
+    Serial.println("Failed to detect and initialize magnetometer!");
+    while (1);
+  }
+
+  mag.enableDefault();
+}
+
+void loop()
+{
+  mag.read();
+
+  snprintf(report, sizeof(report), "M: %6d %6d %6d",
+    mag.m.x, mag.m.y, mag.m.z);
+  Serial.println(report);
+
+  delay(100);
+}
diff --git a/debug/MCP23017/MCP23017.ino b/debug/MCP23017/MCP23017.ino
new file mode 100644
index 0000000..d295e7c
--- /dev/null
+++ b/debug/MCP23017/MCP23017.ino
@@ -0,0 +1,177 @@
+#include <Adafruit_MCP23X17.h>
+
+#define ir1  0   // GPA0
+#define ir2  1   // GPA1
+#define ir3  2   // GPA2
+#define ir4  3   // GPA3
+#define ir5  4   // GPA4  
+#define ir6  5   // GPA5
+#define ir7  6   // GPA6
+#define ir8  7   // GPA7
+#define ir9  8   // GPB0  
+#define ir10 9   // GPB1
+#define ir11 10  // GPB2
+#define ir12 11  // GPB3
+#define ir13 12  // GPB4
+#define ir14 13  // GPB5
+#define ir15 14  // GPB6
+#define ir16 15  // GPB7
+
+int dir1 = 1;   // data IR1
+int dir2 = 1;   // data IR2
+int dir3 = 1;   // etc
+int dir4 = 1;   
+int dir5 = 1;    
+int dir6 = 1;   
+int dir7 = 1;   
+int dir8 = 1;   
+int dir9 = 1;     
+int dir10 = 1;  
+int dir11 = 1; 
+int dir12 = 1; 
+int dir13 = 1; 
+int dir14 = 1; 
+int dir15 = 1; 
+int dir16 = 1; 
+
+int dir17 = 1;   // data IR17
+int dir18 = 1;   // data IR18
+int dir19 = 1;   // etc
+int dir20 = 1;   
+int dir21 = 1;    
+int dir22 = 1;   
+int dir23 = 1;   
+int dir24 = 1;   
+int dir25 = 1;     
+int dir26 = 1;  
+int dir27 = 1; 
+int dir28 = 1; 
+int dir29 = 1; 
+int dir30 = 1; 
+int dir31 = 1; 
+int dir32 = 1; 
+
+// Instance for the first MCP23017 at address 0x20
+Adafruit_MCP23X17 mcp1;
+
+// Instance for the second MCP23017 at address 0x22
+Adafruit_MCP23X17 mcp2;
+
+void setup() {
+  Serial.begin(9600);
+  while (!Serial);
+  Serial.println("MCP23xxx Button Test!");
+
+  // Initialize first MCP23017 with I2C address 0x20
+  if (!mcp1.begin_I2C(0x20)) {
+    Serial.println("Error initializing MCP23017 at 0x20.");
+    while (1);
+  }
+
+  // Initialize second MCP23017 with I2C address 0x22
+  if (!mcp2.begin_I2C(0x22)) {
+    Serial.println("Error initializing MCP23017 at 0x22.");
+    while (1);
+  }
+
+  // Configure pins for the first MCP23017 (0x20)
+  mcp1.pinMode(ir1, INPUT);
+  mcp1.pinMode(ir2, INPUT);
+  mcp1.pinMode(ir3, INPUT);
+  mcp1.pinMode(ir4, INPUT);
+  mcp1.pinMode(ir5, INPUT);
+  mcp1.pinMode(ir6, INPUT);
+  mcp1.pinMode(ir7, INPUT);
+  mcp1.pinMode(ir8, INPUT);
+  mcp1.pinMode(ir9, INPUT);
+  mcp1.pinMode(ir10, INPUT);
+  mcp1.pinMode(ir11, INPUT);
+  mcp1.pinMode(ir12, INPUT);
+  mcp1.pinMode(ir13, INPUT);
+  mcp1.pinMode(ir14, INPUT);
+  mcp1.pinMode(ir15, INPUT);
+  mcp1.pinMode(ir16, INPUT);
+
+  // Configure pins for the second MCP23017 (0x22)
+  mcp2.pinMode(ir1, INPUT);
+  mcp2.pinMode(ir2, INPUT);
+  mcp2.pinMode(ir3, INPUT);
+  mcp2.pinMode(ir4, INPUT);
+  mcp2.pinMode(ir5, INPUT);
+  mcp2.pinMode(ir6, INPUT);
+  mcp2.pinMode(ir7, INPUT);
+  mcp2.pinMode(ir8, INPUT);
+  mcp2.pinMode(ir9, INPUT);
+  mcp2.pinMode(ir10, INPUT);
+  mcp2.pinMode(ir11, INPUT);
+  mcp2.pinMode(ir12, INPUT);
+  mcp2.pinMode(ir13, INPUT);
+  mcp2.pinMode(ir14, INPUT);
+  mcp2.pinMode(ir15, INPUT);
+  mcp2.pinMode(ir16, INPUT);
+
+  Serial.println("Looping...");
+}
+
+void loop() {
+  // Read pins from the first MCP23017 (0x20)
+  dir1 = mcp1.digitalRead(ir1);
+  dir2 = mcp1.digitalRead(ir2);
+  dir3 = mcp1.digitalRead(ir3);
+  dir4 = mcp1.digitalRead(ir4);
+  dir5 = mcp1.digitalRead(ir5);
+  dir6 = mcp1.digitalRead(ir6);
+  dir7 = mcp1.digitalRead(ir7);
+  dir8 = mcp1.digitalRead(ir8);
+  dir9 = mcp1.digitalRead(ir9);
+  dir10 = mcp1.digitalRead(ir10);
+  dir11 = mcp1.digitalRead(ir11);
+  dir12 = mcp1.digitalRead(ir12);
+  dir13 = mcp1.digitalRead(ir13);
+  dir14 = mcp1.digitalRead(ir14);
+  dir15 = mcp1.digitalRead(ir15);
+  dir16 = mcp1.digitalRead(ir16);
+  
+  // Read pins from the second MCP23017 (0x22)
+  dir17 = mcp2.digitalRead(ir1);
+  dir18 = mcp2.digitalRead(ir2);
+  dir19 = mcp2.digitalRead(ir3);
+  dir20 = mcp2.digitalRead(ir4);
+  dir21 = mcp2.digitalRead(ir5);
+  dir22 = mcp2.digitalRead(ir6);
+  dir23 = mcp2.digitalRead(ir7);
+  dir24 = mcp2.digitalRead(ir8);
+  dir25 = mcp2.digitalRead(ir9);
+  dir26 = mcp2.digitalRead(ir10);
+  dir27 = mcp2.digitalRead(ir11);
+  dir28 = mcp2.digitalRead(ir12);
+  dir29 = mcp2.digitalRead(ir13);
+  dir30 = mcp2.digitalRead(ir14);
+  dir31 = mcp2.digitalRead(ir15);
+  dir32 = mcp2.digitalRead(ir16);
+  
+  // Print the results from the first MCP23017 (0x20)
+  Serial.print("MCP23017 @ 0x20: ");
+  Serial.print(dir1); Serial.print(", "); Serial.print(dir2);
+  Serial.print(", "); Serial.print(dir3); Serial.print(", "); Serial.print(dir4);
+  Serial.print(", "); Serial.print(dir5); Serial.print(", "); Serial.print(dir6);
+  Serial.print(", "); Serial.print(dir7); Serial.print(", "); Serial.print(dir8);
+  Serial.print(", "); Serial.print(dir9); Serial.print(", "); Serial.print(dir10);
+  Serial.print(", "); Serial.print(dir11); Serial.print(", "); Serial.print(dir12);
+  Serial.print(", "); Serial.print(dir13); Serial.print(", "); Serial.print(dir14);
+  Serial.print(", "); Serial.print(dir15); Serial.print(", "); Serial.print(dir16);
+
+  // Print the results from the second MCP23017 (0x22)
+  Serial.print("  MCP23017 @ 0x22: ");
+  Serial.print(dir17); Serial.print(", "); Serial.print(dir18);
+  Serial.print(", "); Serial.print(dir19); Serial.print(", "); Serial.print(dir20);
+  Serial.print(", "); Serial.print(dir21); Serial.print(", "); Serial.print(dir22);
+  Serial.print(", "); Serial.print(dir23); Serial.print(", "); Serial.print(dir24);
+  Serial.print(", "); Serial.print(dir25); Serial.print(", "); Serial.print(dir26);
+  Serial.print(", "); Serial.print(dir27); Serial.print(", "); Serial.print(dir28);
+  Serial.print(", "); Serial.print(dir29); Serial.print(", "); Serial.print(dir30);
+  Serial.print(", "); Serial.print(dir31); Serial.print(", "); Serial.println(dir32);
+  
+  Serial.println();
+  delay(250);
+}
diff --git a/debug/MCP23017/mcp20317_schemss.jpg b/debug/MCP23017/mcp20317_schemss.jpg
new file mode 100644
index 0000000..5231394
--- /dev/null
+++ b/debug/MCP23017/mcp20317_schemss.jpg
diff --git a/debug/color_sensor/color_sensor.ino b/debug/color_sensor/color_sensor.ino
new file mode 100644
index 0000000..f3e9c07
--- /dev/null
+++ b/debug/color_sensor/color_sensor.ino
@@ -0,0 +1,183 @@
+/**

+i2c port scanner based on:

+  in wire library examples i2c_scanner

+  https://learn.adafruit.com/adafruit-pca9548-8-channel-stemma-qt-qwiic-i2c-multiplexer/arduino

+

+color sensor based on±

+  adafruit TCS34725 librarys

+

+ type var:

+  https://arduino.stackexchange.com/questions/3079/how-to-retrieve-the-data-type-of-a-variable

+  

+ *

+ */

+

+#include <Wire.h>                  //https://www.arduino.cc/reference/en/language/functions/communication/wire/

+#include "Adafruit_TCS34725.h"    //https://github.com/adafruit/Adafruit_TCS34725

+

+#define PCAADDR 0x70    //i2c address of the i2c multiplexer

+

+/* Initialise with default values (int time = 2.4ms, gain = 1x) */

+Adafruit_TCS34725 tcs = Adafruit_TCS34725();

+//Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_614MS, TCS34725_GAIN_1X);

+

+const int buzzer = 6; //buzzer to arduino pin 9

+

+

+int colsensdata[6] = {99, 99, 99, 99, 99, 99};

+

+

+

+void setup() {

+  Serial.begin(115200);

+  //while (!Serial); // Leonardo: wait for Serial Monitor

+  Serial.println("\nI2C Scanner");

+  pinMode(buzzer, OUTPUT);

+

+}

+

+void pcaselect(uint8_t i) {

+  if (i > 7) return;

+ 

+  Wire.beginTransmission(PCAADDR);

+  Wire.write(1 << i);

+  Wire.endTransmission();  

+}

+void scandirectports(){

+  Wire.begin();

+  int nDevices = 0;

+  Serial.println("Scanning for i2c ports on board...");

+

+  for (byte address = 1; address < 127; ++address) {

+    // The i2c_scanner uses the return value of

+    // the Wire.endTransmission to see if

+    // a device did acknowledge to the address.

+    Wire.beginTransmission(address);

+    byte error = Wire.endTransmission();

+

+    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");

+  }

+}

+

+

+// Generic catch-all implementation.

+template <typename T_ty> struct TypeInfo { static const char * name; };

+template <typename T_ty> const char * TypeInfo<T_ty>::name = "unknown";

+

+// Handy macro to make querying stuff easier.

+#define TYPE_NAME(var) TypeInfo< typeof(var) >::name

+

+// Handy macro to make defining stuff easier.

+#define MAKE_TYPE_INFO(type)  template <> const char * TypeInfo<type>::name = #type;

+

+// Type-specific implementations.

+MAKE_TYPE_INFO( int )

+MAKE_TYPE_INFO( float )

+MAKE_TYPE_INFO( short )

+

+void loop() {

+  Wire.begin();

+  

+  for (uint8_t t=0; t<50; t++) {

+    Serial.println(); 

+  }

+  

+  scandirectports();

+  

+  Serial.println();

+  Serial.println();

+  

+  for (uint8_t t=0; t<6; t++) {

+    pcaselect(t);

+    Serial.print("PCA Port #"); Serial.println(t);

+    for (uint8_t addr = 0; addr<=127; addr++) {

+      if (addr == PCAADDR) continue;

+      Wire.beginTransmission(addr);

+      if (!Wire.endTransmission()) {

+        Serial.print("Found I2C 0x");  Serial.println(addr,HEX);

+        Serial.println("scanning for tcs sensor");

+        if (tcs.begin()) {

+          Serial.println("Found sensor");

+          Serial.println("collecting data");

+          uint16_t r, g, b, c, colorTemp, lux;

+          tcs.getRawData(&r, &g, &b, &c);

+          colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);

+          lux = tcs.calculateLux(r, g, b);

+          Serial.print("Color Temp: "); Serial.print(colorTemp, DEC); Serial.print(" K - ");

+          Serial.print("RGB: "); Serial.print(r, DEC); Serial.print(", "); Serial.print(g, DEC); Serial.print(", "); Serial.print(b, DEC); Serial.println(" ");

+          if ((colorTemp) == 5201){

+            Serial.println("sensor in air");

+            tone(buzzer, 100);

+            colsensdata[t] = 0;

+          }

+          else if ((g)>=(2*(r)) && (g)>=12){

+            Serial.println("green");  

+            tone(buzzer, 200);

+            colsensdata[t] = 1; 

+          }

+          else if ((colorTemp)>= 6750){

+            Serial.println("black"); 

+            tone(buzzer, 300);

+            colsensdata[t] = 2; 

+          }

+          else if ((colorTemp) < 6750){

+            Serial.println("white");  

+            tone(buzzer, 400);

+            colsensdata[t] = 3;

+          }

+          else{

+            Serial.println(colorTemp);

+            Serial.println(TYPE_NAME((colorTemp, DEC)));

+            colsensdata[t] = 98;

+          }

+          Serial.println();

+

+          

+        } else {

+          Serial.println("No TCS34725 found ... check your connections");

+          colsensdata[t] = 97;

+        }

+        delay(500);

+      }

+    }

+  }

+  Serial.println();

+  Serial.print("sensor data = ");

+  for(int i = 0; i < 6; i++){

+  Serial.print(colsensdata[i]);

+  Serial.print(", ");

+  }

+  /**

+0 = air

+1 = green

+2 = black

+3 = white

+90 = error

+   

+   * 

+   */

+  Serial.println("");

+  Serial.println("DONE");

+  tone(buzzer, 1000);

+  delay(2500);

+}

diff --git a/debug/color_sensor/short/short.ino b/debug/color_sensor/short/short.ino
new file mode 100644
index 0000000..2f80412
--- /dev/null
+++ b/debug/color_sensor/short/short.ino
@@ -0,0 +1,94 @@
+
+#include <Wire.h>                  //https://www.arduino.cc/reference/en/language/functions/communication/wire/
+#include "Adafruit_TCS34725.h"    //https://github.com/adafruit/Adafruit_TCS34725
+
+#define PCAADDR 0x70    //i2c address of the i2c multiplexer
+
+
+Adafruit_TCS34725 tcs = Adafruit_TCS34725();/* Initialise with default values (int time = 2.4ms, gain = 1x) */
+
+int colsensdata[6] = {99, 99, 99, 99, 99, 99};
+
+void setup() {
+  Serial.begin(115200);
+  Serial.println("\nI2C Scanner");
+
+}
+
+void pcaselect(uint8_t i) {
+  if (i > 7) return;
+ 
+  Wire.beginTransmission(PCAADDR);
+  Wire.write(1 << i);
+  Wire.endTransmission();  
+}
+
+template <typename T_ty> struct TypeInfo { static const char * name; };
+template <typename T_ty> const char * TypeInfo<T_ty>::name = "unknown";
+#define TYPE_NAME(var) TypeInfo< typeof(var) >::name
+#define MAKE_TYPE_INFO(type)  template <> const char * TypeInfo<type>::name = #type;
+MAKE_TYPE_INFO( int )
+MAKE_TYPE_INFO( float )
+MAKE_TYPE_INFO( short )
+
+void loop() {
+  Wire.begin();
+  for (uint8_t t=0; t<6; t++) {
+    pcaselect(t);
+    Serial.print("PCA Port #"); Serial.println(t);
+    for (uint8_t addr = 0; addr<=127; addr++) {
+      if (addr == PCAADDR) continue;
+      Wire.beginTransmission(addr);
+      if (!Wire.endTransmission()) {
+        Serial.print("Found I2C 0x");  Serial.println(addr,HEX);
+        if (tcs.begin()) {
+          Serial.println("Found sensor");
+          Serial.println("collecting data");
+          uint16_t r, g, b, c, colorTemp, lux;
+          tcs.getRawData(&r, &g, &b, &c);
+          colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);
+          lux = tcs.calculateLux(r, g, b);
+          Serial.print("Color Temp: "); Serial.print(colorTemp, DEC); Serial.print(" K - ");
+          Serial.print("RGB: "); Serial.print(r, DEC); Serial.print(", "); Serial.print(g, DEC); Serial.print(", "); Serial.print(b, DEC); Serial.println(" ");
+          if ((colorTemp) == 5201){
+            colsensdata[t] = 0;
+          }
+          else if ((g)>=(2*(r)) && (g)>=12){
+            colsensdata[t] = 1; 
+          }
+          else if ((colorTemp)>= 6750){
+            colsensdata[t] = 2; 
+          }
+          else if ((colorTemp) < 6750){
+            colsensdata[t] = 3;
+          }
+          else{
+            Serial.println(colorTemp);
+            Serial.println(TYPE_NAME((colorTemp, DEC)));
+            colsensdata[t] = 98;
+          }
+        } else {
+          Serial.println("No TCS34725 found ... check your connections");
+          colsensdata[t] = 97;
+        }
+      }
+    }
+  }
+  Serial.print("sensor data = ");
+  for(int i = 0; i < 6; i++){
+    Serial.print(colsensdata[i]);
+    Serial.print(", ");
+  }
+  /**
+0 = air
+1 = green
+2 = black
+3 = white
+90 = error
+   
+   * 
+   */
+  Serial.println("");
+  Serial.println("DONE");
+  delay(2500);
+}
\ No newline at end of file
diff --git a/debug/comm/arduino_binairy_decoder/arduino_binairy_decoder.ino b/debug/comm/arduino_binairy_decoder/arduino_binairy_decoder.ino
new file mode 100644
index 0000000..eb257e5
--- /dev/null
+++ b/debug/comm/arduino_binairy_decoder/arduino_binairy_decoder.ino
@@ -0,0 +1,28 @@
+String inp = "01101000";
+int decimalValue = 0;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for native USB
+  }
+  Serial.println("");
+
+  Serial.println(inp);
+  String binary = inp.substring(2);
+  Serial.println(binary);
+  for (int i = 0; i < binary.length(); i++) {
+    // Calculate the value of the current bit
+    int bitValue = binary.charAt(i) - '0';
+    // Update the decimal value by shifting the bit to its correct position
+    decimalValue = (decimalValue << 1) | bitValue;
+  }
+  Serial.println(decimalValue);
+  Serial.println("");
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  delay(1000);
+
+}
diff --git a/debug/comm/arduino_comm/arduino_comm.ino b/debug/comm/arduino_comm/arduino_comm.ino
new file mode 100644
index 0000000..4a2b205
--- /dev/null
+++ b/debug/comm/arduino_comm/arduino_comm.ino
@@ -0,0 +1,37 @@
+String tlocd = "";
+String locd = "";
+bool readloc = false;
+unsigned long datatime = 0;
+
+void setup() {
+  Serial1.begin(19200);
+  Serial.begin(19200); 
+}
+
+void loop() {
+  if(Serial1.available()){
+    datatime = millis(); 
+
+    byte byteRead = Serial1.read();
+    
+    if(byteRead == 's'){
+      tlocd = ""; 
+      readloc = true;
+    }
+    else if(byteRead == 'e'){
+      readloc = false;
+      locd = tlocd;
+    }
+    else if(readloc == true){
+      tlocd += (char)byteRead;
+    }
+  }
+  else{
+    if(millis() - datatime > 5000){
+      Serial.println("Err no data recieved for 5 sec");  
+      locd = "999";
+    }
+  }
+  Serial.println(locd);
+  delay(134);
+}
diff --git a/debug/comm/deg_to_byte.py b/debug/comm/deg_to_byte.py
new file mode 100644
index 0000000..196d4a5
--- /dev/null
+++ b/debug/comm/deg_to_byte.py
@@ -0,0 +1,5 @@
+deg = 40
+sbinary = format(deg, 'b')
+print(sbinary)
+binary = "".join(("01", sbinary))
+print(binary)
\ No newline at end of file
diff --git a/debug/comm/i2c/arduino_i2c_slave_1.py b/debug/comm/i2c/arduino_i2c_slave_1.py
new file mode 100644
index 0000000..5957c5b
--- /dev/null
+++ b/debug/comm/i2c/arduino_i2c_slave_1.py
@@ -0,0 +1,98 @@
+# This work is licensed under the MIT license.
+# Copyright (c) 2013-2023 OpenMV LLC. All rights reserved.
+# https://github.com/openmv/openmv/blob/master/LICENSE
+#
+# I2C with the Arduino as the master device and the OpenMV Cam as the slave.
+#
+# Please wire up your OpenMV Cam to your Arduino like this:
+#
+# OpenMV Cam Master I2C Data  (P5) - Arduino Uno Data  (A4)
+# OpenMV Cam Master I2C Clock (P4) - Arduino Uno Clock (A5)
+# OpenMV Cam Ground                - Arduino Ground
+
+import pyb
+import ustruct
+
+text = "Hello World!\n"
+data = ustruct.pack("<%ds" % len(text), text)
+# Use "ustruct" to build data packets to send.
+# "<" puts the data in the struct in little endian order.
+# "%ds" puts a string in the data stream. E.g. "13s" for "Hello World!\n" (13 chars).
+# See https://docs.python.org/3/library/struct.html
+
+# READ ME!!!
+#
+# Please understand that when your OpenMV Cam is not the I2C master it may miss responding to
+# sending data as a I2C slave no matter if you call "i2c.send()" in an interrupt callback or in the
+# main loop below. When this happens the Arduino will get a NAK and have to try reading from the
+# OpenMV Cam again. Note that both the Arduino and OpenMV Cam I2C drivers are not good at getting
+# unstuck after encountering any I2C errors. On the OpenMV Cam and Arduino you can recover by
+# de-initing and then re-initing the I2C peripherals.
+
+# The hardware I2C bus for your OpenMV Cam is always I2C bus 2.
+bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=0x12)
+bus.deinit()  # Fully reset I2C device...
+bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=0x12)
+print("Waiting for Arduino...")
+
+# Note that for sync up to work correctly the OpenMV Cam must be running this script before the
+# Arduino starts to poll the OpenMV Cam for data. Otherwise the I2C byte framing gets messed up,
+# and etc. So, keep the Arduino in reset until the OpenMV Cam is "Waiting for Arduino...".
+
+while True:
+    try:
+        bus.send(
+            ustruct.pack("<h", len(data)), timeout=10000
+        )  # Send the len first (16-bits).
+        try:
+            bus.send(data, timeout=10000)  # Send the data second.
+            print("Sent Data!")  # Only reached on no error.
+        except OSError as err:
+            pass  # Don't care about errors - so pass.
+            # Note that there are 3 possible errors. A timeout error, a general purpose error, or
+            # a busy error. The error codes are 116, 5, 16 respectively for "err.arg[0]".
+    except OSError as err:
+        pass  # Don't care about errors - so pass.
+        # Note that there are 3 possible errors. A timeout error, a general purpose error, or
+        # a busy error. The error codes are 116, 5, 16 respectively for "err.arg[0]".
+
+###################################################################################################
+# Arduino Code
+###################################################################################################
+#
+# #include <Wire.h>
+# #define BAUD_RATE 19200
+# #define CHAR_BUF 128
+#
+# void setup() {
+#   Serial.begin(BAUD_RATE);
+#   Wire.begin();
+#   delay(1000); // Give the OpenMV Cam time to bootup.
+# }
+#
+# void loop() {
+#   int32_t temp = 0;
+#   char buff[CHAR_BUF] = {0};
+#
+#   Wire.requestFrom(0x12, 2);
+#   if(Wire.available() == 2) { // got length?
+#
+#     temp = Wire.read() | (Wire.read() << 8);
+#     delay(1); // Give some setup time...
+#
+#     Wire.requestFrom(0x12, temp);
+#     if(Wire.available() == temp) { // got full message?
+#
+#       temp = 0;
+#       while(Wire.available()) buff[temp++] = Wire.read();
+#
+#     } else {
+#       while(Wire.available()) Wire.read(); // Toss garbage bytes.
+#     }
+#   } else {
+#     while(Wire.available()) Wire.read(); // Toss garbage bytes.
+#   }
+#
+#   Serial.print(buff);
+#   delay(1); // Don't loop to quickly.
+# }
diff --git a/debug/comm/i2c/i2c_arduino_master/i2c_arduino_master.ino b/debug/comm/i2c/i2c_arduino_master/i2c_arduino_master.ino
new file mode 100644
index 0000000..ff960ed
--- /dev/null
+++ b/debug/comm/i2c/i2c_arduino_master/i2c_arduino_master.ino
@@ -0,0 +1,36 @@
+#include <Wire.h>
+#define BAUD_RATE 19200
+#define CHAR_BUF 128
+
+void setup() {
+  Serial.begin(BAUD_RATE);
+  Wire.begin();
+  delay(1000); // Give the OpenMV Cam time to bootup.
+}
+
+void loop() {
+  int32_t temp = 0;
+  char buff[CHAR_BUF] = {0};
+
+  Wire.requestFrom(0x12, 2);
+  if(Wire.available() == 2) { // got length?
+
+    temp = Wire.read() | (Wire.read() << 8);
+    delay(1); // Give some setup time...
+
+    Wire.requestFrom(0x12, temp);
+    if(Wire.available() == temp) { // got full message?
+
+      temp = 0;
+      while(Wire.available()) buff[temp++] = Wire.read();
+
+    } else {
+      while(Wire.available()) Wire.read(); // Toss garbage bytes.
+    }
+  } else {
+    while(Wire.available()) Wire.read(); // Toss garbage bytes.
+  }
+
+  Serial.print(buff);
+  delay(1); // Don't loop to quickly.
+}
diff --git a/debug/comm/openmv_com.py b/debug/comm/openmv_com.py
new file mode 100644
index 0000000..d5a3faf
--- /dev/null
+++ b/debug/comm/openmv_com.py
@@ -0,0 +1,13 @@
+import time
+from pyb import UART
+
+# UART 3, and baudrate.
+uart = UART(3, 19200, timeout_char=200)
+location = "40"  #place to put the goal location
+while True:
+    uart.write("s")
+    uart.write(location)
+    uart.write("e")
+    if uart.any():
+        print(uart.read())
+    time.sleep_ms(1000)
diff --git a/debug/comm/spi/arduino_spi_master/arduino_spi_master.ino b/debug/comm/spi/arduino_spi_master/arduino_spi_master.ino
new file mode 100644
index 0000000..248ef0f
--- /dev/null
+++ b/debug/comm/spi/arduino_spi_master/arduino_spi_master.ino
@@ -0,0 +1,34 @@
+#include <SPI.h>
+#define SS_PIN 10
+#define BAUD_RATE 19200
+#define CHAR_BUF 128
+
+void setup() {
+  pinMode(SS_PIN, OUTPUT);
+  Serial.begin(BAUD_RATE);
+  SPI.begin();
+  SPI.setBitOrder(MSBFIRST);
+  SPI.setClockDivider(SPI_CLOCK_DIV16);
+  SPI.setDataMode(SPI_MODE0);
+  delay(1000); // Give the OpenMV Cam time to bootup.
+}
+
+void loop() {
+  int32_t len = 0;
+  char buff[CHAR_BUF] = {0};
+  digitalWrite(SS_PIN, LOW);
+  delay(1); // Give the OpenMV Cam some time to setup to send data.
+
+  if(SPI.transfer(1) == 85) { // saw sync char?
+    SPI.transfer(&len, 4); // get length
+    if (len) {
+      SPI.transfer(&buff, min(len, CHAR_BUF));
+      len -= min(len, CHAR_BUF);
+    }
+    while (len--) SPI.transfer(0); // eat any remaining bytes
+  }
+
+  digitalWrite(SS_PIN, HIGH);
+  Serial.print(buff);
+  delay(1); // Don't loop to quickly.
+}
\ No newline at end of file
diff --git a/debug/comm/spi/arduino_spi_slave_1.py b/debug/comm/spi/arduino_spi_slave_1.py
new file mode 100644
index 0000000..e05c2f0
--- /dev/null
+++ b/debug/comm/spi/arduino_spi_slave_1.py
@@ -0,0 +1,102 @@
+# This work is licensed under the MIT license.
+# Copyright (c) 2013-2023 OpenMV LLC. All rights reserved.
+# https://github.com/openmv/openmv/blob/master/LICENSE
+#
+# SPI with the Arduino as the master device and the OpenMV Cam as the slave.
+#
+# Please wire up your OpenMV Cam to your Arduino like this:
+#
+# OpenMV Cam Master Out Slave In (P0) - Arduino Uno MOSI (11)
+# OpenMV Cam Master In Slave Out (P1) - Arduino Uno MISO (12)
+# OpenMV Cam Serial Clock        (P2) - Arduino Uno SCK  (13)
+# OpenMV Cam Slave Select        (P3) - Arduino Uno SS   (10)
+# OpenMV Cam Ground                   - Arduino Ground
+
+import pyb
+import ustruct
+import time
+
+text = "Hello World!\n"
+data = ustruct.pack("<bi%ds" % len(text), 85, len(text), text)  # 85 is a sync char.
+# Use "ustruct" to build data packets to send.
+# "<" puts the data in the struct in little endian order.
+# "b" puts a signed char in the data stream.
+# "i" puts a signed integer in the data stream.
+# "%ds" puts a string in the data stream. E.g. "13s" for "Hello World!\n" (13 chars).
+# See https://docs.python.org/3/library/struct.html
+
+# READ ME!!!
+#
+# Please understand that when your OpenMV Cam is not the SPI master it may miss responding to
+# sending data as a SPI slave no matter if you call "spi.send()" in an interrupt callback or in the
+# main loop below. Because of this you must absolutely design your communications protocol such
+# that if the slave device (the OpenMV Cam) doesn't call "spi.send()" in time that garbage data
+# read from the SPI peripheral by the master (the Arduino) is tossed. To accomplish this we use a
+# sync character of 85 (binary 01010101) which the Arduino will look for as the first byte read.
+# If it doesn't see this then it aborts the SPI transaction and will try again. Second, in order to
+# clear out the SPI peripheral state we always send a multiple of four bytes and an extra four zero
+# bytes to make sure that the SPI peripheral doesn't hold any stale data which could be 85. Note
+# that the OpenMV Cam will miss the "spi.send()" window randomly because it has to service
+# interrupts. Interrupts will happen a lot more while connected to your PC.
+
+# The hardware SPI bus for your OpenMV Cam is always SPI bus 2.
+# polarity = 0 -> clock is idle low.
+# phase = 0 -> sample data on rising clock edge, output data on falling clock edge.
+spi = pyb.SPI(2, pyb.SPI.SLAVE, polarity=0, phase=0)
+
+
+# NSS callback.
+def nss_callback(line):
+    global spi, data
+    try:
+        spi.send(data, timeout=1000)
+    except OSError as err:
+        pass  # Don't care about errors - so pass.
+        # Note that there are 3 possible errors. A timeout error, a general purpose error, or
+        # a busy error. The error codes are 116, 5, 16 respectively for "err.arg[0]".
+
+
+# Configure NSS/CS in IRQ mode to send data when requested by the master.
+pyb.ExtInt(pyb.Pin("P3"), pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, nss_callback)
+
+while True:
+    time.sleep_ms(1000)
+
+###################################################################################################
+# Arduino Code
+###################################################################################################
+#
+# #include <SPI.h>
+# #define SS_PIN 10
+# #define BAUD_RATE 19200
+# #define CHAR_BUF 128
+#
+# void setup() {
+#   pinMode(SS_PIN, OUTPUT);
+#   Serial.begin(BAUD_RATE);
+#   SPI.begin();
+#   SPI.setBitOrder(MSBFIRST);
+#   SPI.setClockDivider(SPI_CLOCK_DIV16);
+#   SPI.setDataMode(SPI_MODE0);
+#   delay(1000); // Give the OpenMV Cam time to bootup.
+# }
+#
+# void loop() {
+#   int32_t len = 0;
+#   char buff[CHAR_BUF] = {0};
+#   digitalWrite(SS_PIN, LOW);
+#   delay(1); // Give the OpenMV Cam some time to setup to send data.
+#
+#   if(SPI.transfer(1) == 85) { // saw sync char?
+#     SPI.transfer(&len, 4); // get length
+#     if (len) {
+#       SPI.transfer(&buff, min(len, CHAR_BUF));
+#       len -= min(len, CHAR_BUF);
+#     }
+#     while (len--) SPI.transfer(0); // eat any remaining bytes
+#   }
+#
+#   digitalWrite(SS_PIN, HIGH);
+#   Serial.print(buff);
+#   delay(1); // Don't loop to quickly.
+# }
diff --git a/debug/comm/uart/arduino_uart/arduino_uart.ino b/debug/comm/uart/arduino_uart/arduino_uart.ino
new file mode 100644
index 0000000..6385695
--- /dev/null
+++ b/debug/comm/uart/arduino_uart/arduino_uart.ino
@@ -0,0 +1,17 @@
+void setup() {
+  // Initialize Serial1 with a baud rate of 19200
+  Serial1.begin(19200);
+  Serial.begin(19200); // Initialize Serial for debugging
+}
+
+void loop() {
+  // Check if there is any data available to read from Serial1
+  if (Serial1.available()) {
+    // Read the most recent byte
+    byte byteRead = Serial1.read();
+    // Echo the value that was read
+    Serial1.write(byteRead);
+    // Print the received byte for debugging
+    Serial.print((char)byteRead);
+  }
+}
diff --git a/debug/comm/uart/arduino_uart_1.py b/debug/comm/uart/arduino_uart_1.py
new file mode 100644
index 0000000..3996cf7
--- /dev/null
+++ b/debug/comm/uart/arduino_uart_1.py
@@ -0,0 +1,42 @@
+# This work is licensed under the MIT license.
+# Copyright (c) 2013-2023 OpenMV LLC. All rights reserved.
+# https://github.com/openmv/openmv/blob/master/LICENSE
+#
+# Basic UART communications between OpenMV and Arduino Uno.
+
+# 1) Wire up your OpenMV Cam to your Arduino Uno like this:
+#
+# OpenMV Cam Ground Pin   ----> Arduino Ground
+# OpenMV Cam UART3_TX(P4) ----> Arduino Uno UART_RX(0)
+# OpenMV Cam UART3_RX(P5) ----> Arduino Uno UART_TX(1)
+
+# 2) Uncomment and upload the following sketch to Arduino:
+#
+# void setup() {
+#   // put your setup code here, to run once:
+#   Serial.begin(19200);
+# }
+#
+# void loop() {
+#   // put your main code here, to run repeatedly:
+#   if (Serial.available()) {
+#     // Read the most recent byte
+#     byte byteRead = Serial.read();
+#     // ECHO the value that was read
+#     Serial.write(byteRead);
+#   }
+# }
+
+# 3) Run the following script in OpenMV IDE:
+
+import time
+from pyb import UART
+
+# UART 3, and baudrate.
+uart = UART(3, 19200, timeout_char=200)
+
+while True:
+    uart.write("Hello World!\n")
+    if uart.any():
+        print(uart.read())
+    time.sleep_ms(1000)
diff --git a/debug/i2c_scanner/i2c_scanner.ino b/debug/i2c_scanner/i2c_scanner.ino
new file mode 100644
index 0000000..8b13954
--- /dev/null
+++ b/debug/i2c_scanner/i2c_scanner.ino
@@ -0,0 +1,93 @@
+/**

+i2c port scanner based on:

+  in wire library examples i2c_scanner

+  https://learn.adafruit.com/adafruit-pca9548-8-channel-stemma-qt-qwiic-i2c-multiplexer/arduino

+

+

+ *

+ */

+

+#include <Wire.h>

+

+#define PCAADDR 0x70    //i2c address of the i2c multiplexer

+

+

+void setup() {

+  Serial.begin(115200);

+  while (!Serial); // Leonardo: wait for Serial Monitor

+  Serial.println("\nI2C Scanner");

+

+}

+

+void pcaselect(uint8_t i) {

+  if (i > 7) return;

+ 

+  Wire.beginTransmission(PCAADDR);

+  Wire.write(1 << i);

+  Wire.endTransmission();  

+}

+

+

+

+

+void scandirectports(){

+  Wire.begin();

+  int nDevices = 0;

+  Serial.println("Scanning for i2c ports on board...");

+

+  for (byte address = 1; address < 127; ++address) {

+    // The i2c_scanner uses the return value of

+    // the Wire.endTransmission to see if

+    // a device did acknowledge to the address.

+    Wire.beginTransmission(address);

+    byte error = Wire.endTransmission();

+

+    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");

+  }

+  Serial.println("done scanning for i2c ports on board");

+}

+void scani2cmultiplexer(){

+  Serial.println("Scanning for i2c ports on i2c multiplexer...");

+  for (uint8_t t=0; t<8; t++) {

+    pcaselect(t);

+    Serial.print("PCA Port #"); Serial.println(t);

+

+    for (uint8_t addr = 0; addr<=127; addr++) {

+      if (addr == PCAADDR) continue;

+      Wire.beginTransmission(addr);

+      if (!Wire.endTransmission()) {

+        Serial.print("Found I2C 0x");  Serial.println(addr,HEX);

+      }

+    }

+  }

+  Serial.println("done scanning for i2c ports on i2c multiplexer");

+}

+

+

+

+void loop() {

+ scandirectports();

+ delay(1000);

+ scani2cmultiplexer();

+ delay(1000);

+}

diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
new file mode 100644
index 0000000..0fd30e7
--- /dev/null
+++ b/debug/motordriver/motordriver.ino
@@ -0,0 +1,258 @@
+//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
+
+
+#include <Adafruit_MCP23X17.h>
+
+// motor one
+int enA = 33;
+int in1 = 2;
+int in2 = 3;
+// motor two
+int enB = 36;
+int in3 = 4;
+int in4 = 5;
+// motor three
+int enC = 37;
+int in5 = 7;
+int in6 = 6;
+
+int sp = 160;
+
+double dirA = 0.00;
+double dirB = 0.00;
+double dirC = 0.00;
+double spdA = 0.00;
+double spdB = 0.00;
+double spdC = 0.00;
+double dirrad = 0.00;
+
+void setup()
+{
+ // set all the motor control pins to outputs
+ pinMode(enA, OUTPUT);
+ pinMode(enB, OUTPUT);
+ pinMode(enC, OUTPUT);
+
+ pinMode(in1, OUTPUT);
+ pinMode(in2, OUTPUT);
+ pinMode(in3, OUTPUT);
+ pinMode(in4, OUTPUT);
+ pinMode(in5, OUTPUT);
+ pinMode(in6, OUTPUT);
+}
+void demoOne()
+{
+// this function will run the motors in both directions at a fixed speed
+  // turn on motor A
+  digitalWrite(in1, HIGH);
+  digitalWrite(in2, LOW);
+  // set speed to 200 out of possible range 0~255
+  analogWrite(enA, sp);
+  // turn on motor B
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+  // set speed to 200 out of possible range 0~255
+  analogWrite(enB, sp);
+  // turn on motor c
+  digitalWrite(in5, HIGH);
+  digitalWrite(in6, LOW);
+  // set speed to 200 out of possible range 0~255
+  analogWrite(enC, sp);
+  delay(2000);
+  // now change motor directions
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, HIGH);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, HIGH);
+  digitalWrite(in5, LOW);
+  digitalWrite(in6, HIGH);
+  delay(2000);
+  // now turn off motors
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, LOW);
+}
+void demoTwo()
+{
+  // this function will run the motors across the range of possible speeds
+  // note that maximum speed is determined by the motor itself and theoperating voltage
+  // the PWM values sent by analogWrite() are fractions of the maximumspeed possible
+  // by your hardware
+  // turn on motors
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, HIGH);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, HIGH);
+  digitalWrite(in5, LOW);
+  digitalWrite(in6, HIGH);
+  // accelerate from zero to maximum speed
+  for (int i = 0; i < 256; i++)
+  {
+    analogWrite(enA, i);
+    analogWrite(enB, i);
+    analogWrite(enC, i); 
+    delay(20);
+  }
+  // decelerate from maximum speed to zero
+  for (int i = 255; i >= 0; --i)
+  {
+    analogWrite(enA, i);
+    analogWrite(enB, i);
+    analogWrite(enC, i);
+    delay(20);
+  }
+  // now turn off motors
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, LOW);
+  digitalWrite(in6, LOW);
+}
+
+void Amove(int sped){
+  Serial.print("moving A ");
+  if (sped < 0) {
+    digitalWrite(in1, LOW);
+    digitalWrite(in2, HIGH);
+    Serial.print("back");
+  }
+  else{
+    digitalWrite(in1, HIGH);
+    digitalWrite(in2, LOW);
+    Serial.print("forward");
+  } 
+  Serial.print("with speed: ");
+  sp = abs(sped); 
+  analogWrite(enA, sp);
+  Serial.println(sp);
+}
+void Bmove(int sped) {
+  Serial.print("moving B ");
+  if (sped < 0) {
+    digitalWrite(in3, LOW);
+    digitalWrite(in4, HIGH);
+    Serial.print("back");
+  } else {
+    digitalWrite(in3, HIGH);
+    digitalWrite(in4, LOW);
+    Serial.print("forward");
+  } 
+  Serial.print("with speed: ");
+  int sp = abs(sped);
+  analogWrite(enB, sp);
+  Serial.println(sp);
+}
+void Cmove(int sped) {
+  Serial.print("moving C ");
+  if (sped < 0) {
+    digitalWrite(in5, LOW);
+    digitalWrite(in6, HIGH);
+    Serial.print("back");
+  } else {
+    digitalWrite(in5, HIGH);
+    digitalWrite(in6, LOW);
+    Serial.print("forward");
+  } 
+  Serial.print("with speed: ");
+  int sp = abs(sped);
+  analogWrite(enC, sp);
+  Serial.println(sp);
+}
+
+
+void turnoff(){
+  Serial.println("stop");
+  analogWrite(enA, 0);
+  analogWrite(enB, 0);
+  analogWrite(enC, 0);
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, LOW);
+  digitalWrite(in6, LOW); 
+}
+void forward(int speed){
+  Serial.println("forward");
+  analogWrite(enA, speed);
+  analogWrite(enB, speed);
+  analogWrite(enC, speed);
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, HIGH);
+  digitalWrite(in6, LOW);
+}
+void turn(int speed){
+  Serial.println("turn");
+  analogWrite(enA, speed);
+  analogWrite(enB, speed);
+  analogWrite(enC, speed);
+  digitalWrite(in1, HIGH);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, HIGH);
+  digitalWrite(in6, LOW);
+}
+
+void moveohmi(int speed, int direction){
+  turnoff();
+  double dirrad = (direction*71) / 4068;
+  dirA = 1.047198-dirrad; 
+  dirB = 3.141593-dirrad;
+  dirC = 5.235988-dirrad;
+  spdA = speed*sin(dirA);
+  spdB = speed*sin(dirB);
+  spdC = speed*sin(dirC);
+  Amove(spdA);
+  Bmove(spdB);
+  Cmove(spdC);
+}
+void beddermoveohmi(int speed, int direction){
+  turnoff();
+
+  double dirrad = (direction*71) / 4068;  
+  Serial.println(dirrad);
+  //vector ontb in x en y
+  double x = sin(dirrad)*speed;
+  double y = cos(dirrad)*speed;
+  Serial.println((String)"x:"+x+" y:"+y);
+  spdA = -0.5*x - sqrt(3)/2*y;
+  spdB = x;
+  spdC = -0.5*x + sqrt(3)/2*y;
+  Amove(spdA);
+  Bmove(spdB);
+  Cmove(spdC);
+
+  Serial.println(spdA);
+  Serial.println(spdB);
+  Serial.println(spdC);
+  Serial.println();
+  Serial.println();
+}
+void loop()
+{
+  //demoOne();
+  //delay(1000);
+  //demoTwo();
+  delay(1000);
+  turnoff();
+  beddermoveohmi(255, 0);
+  delay(1000);
+  turnoff();
+  Amove(255);
+  delay(1000);
+  turnoff();
+  Bmove(255);
+  delay(1000);
+  turnoff();
+  Cmove(255);
+  delay(1000);
+  turnoff();
+  turn(254);
+  delay(1000);
+}
diff --git a/debug/motordriver/nomultiplexer/nomultiplexer.ino b/debug/motordriver/nomultiplexer/nomultiplexer.ino
new file mode 100644
index 0000000..d2800a8
--- /dev/null
+++ b/debug/motordriver/nomultiplexer/nomultiplexer.ino
@@ -0,0 +1,192 @@
+//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
+//https://www.pololu.com/product/2997
+//https://www.pololu.com/product/4861
+//
+// motor one
+int m1penA = 1;
+int m1enB = 2;
+int m1pwm1 = 3;
+int m1pwm2 = 4;
+int m1encA = 13;
+int m1encB = 14;
+// motor two
+int m2enA = 5;
+int m2enB = 6;
+int m2pwm1 = 7;
+int m2pwm2 = 8;
+// motor three
+int m3enA = 9;
+int m3enB = 10;
+int m3pwm1 = 11;
+int m3pwm2 = 12;
+
+int sp = 160;
+
+double dirA = 0.00;
+double dirB = 0.00;
+double dirC = 0.00;
+double spdA = 0.00;
+double spdB = 0.00;
+double spdC = 0.00;
+double dirrad = 0.00;
+
+void setup()
+{
+ // set all the motor control pins to outputs
+ pinMode(m1enA, OUTPUT);
+ pinMode(m1enB, OUTPUT);
+ pinMode(m2enA, OUTPUT);
+ pinMode(m2enB, OUTPUT);
+ pinMode(m3enA, OUTPUT);
+ pinMode(m3enB, OUTPUT);
+
+ pinMode(m1pwm1, OUTPUT);
+ pinMode(m1pwm2, OUTPUT);
+ pinMode(m2pwm1, OUTPUT);
+ pinMode(m2pwm2, OUTPUT);
+ pinMode(m3pwm1, OUTPUT);
+ pinMode(m3pwm2, OUTPUT);
+
+ pinMode(m1encA, INPUT);
+ pinMode(m1encB, INPUT);
+}
+
+void Amove(int sped){
+  digitalWrite(m1enA, HIGH);
+  digitalWrite(m1enA, LOW);
+  sp = abs(sped);
+  if (sped < 0) {
+    Serial.print("back");
+    analogWrite(m1pwm1, sp);
+    digitalWrite(m1pwm2, LOW);
+  } else {
+    Serial.print("forward");
+    digitalWrite(m1pwm1, LOW);
+    analogWrite(m1pwm2, sp);
+  } 
+  Serial.println(sp);
+}
+void Bmove(int sped) {
+  Serial.print("moving B ");
+  if (sped < 0) {
+    digitalWrite(in3, LOW);
+    digitalWrite(in4, HIGH);
+    Serial.print("back");
+  } else {
+    digitalWrite(in3, HIGH);
+    digitalWrite(in4, LOW);
+    Serial.print("forward");
+  } 
+  Serial.print("with speed: ");
+  int sp = abs(sped);
+  analogWrite(enB, sp);
+  Serial.println(sp);
+}
+void Cmove(int sped) {
+  Serial.print("moving C ");
+  if (sped < 0) {
+    digitalWrite(in5, LOW);
+    digitalWrite(in6, HIGH);
+    Serial.print("back");
+  } else {
+    digitalWrite(in5, HIGH);
+    digitalWrite(in6, LOW);
+    Serial.print("forward");
+  } 
+  Serial.print("with speed: ");
+  int sp = abs(sped);
+  analogWrite(enC, sp);
+  Serial.println(sp);
+}
+
+void turnoff(){
+  Serial.println("stop");
+  analogWrite(enA, 0);
+  analogWrite(enB, 0);
+  analogWrite(enC, 0);
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, LOW);
+  digitalWrite(in6, LOW); 
+}
+void forward(int speed){
+  Serial.println("forward");
+  analogWrite(enA, speed);
+  analogWrite(enB, speed);
+  analogWrite(enC, speed);
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, HIGH);
+  digitalWrite(in6, LOW);
+}
+void turn(int speed){
+  Serial.println("turn");
+  analogWrite(enA, speed);
+  analogWrite(enB, speed);
+  analogWrite(enC, speed);
+  digitalWrite(in1, HIGH);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+  digitalWrite(in5, HIGH);
+  digitalWrite(in6, LOW);
+}
+
+void moveohmi(int speed, int direction){
+  turnoff();
+  double dirrad = (direction*71) / 4068;
+  dirA = 1.047198-dirrad; 
+  dirB = 3.141593-dirrad;
+  dirC = 5.235988-dirrad;
+  spdA = speed*sin(dirA);
+  spdB = speed*sin(dirB);
+  spdC = speed*sin(dirC);
+  Amove(spdA);
+  Bmove(spdB);
+  Cmove(spdC);
+}
+void beddermoveohmi(int speed, int direction){
+  turnoff();
+
+  double dirrad = (direction*71) / 4068;  
+  Serial.println(dirrad);
+  //vector ontb in x en y
+  double x = sin(dirrad)*speed;
+  double y = cos(dirrad)*speed;
+  Serial.println((String)"x:"+x+" y:"+y);
+  spdA = -0.5*x - sqrt(3)/2*y;
+  spdB = x;
+  spdC = -0.5*x + sqrt(3)/2*y;
+  Amove(spdA);
+  Bmove(spdB);
+  Cmove(spdC);
+
+  Serial.println(spdA);
+  Serial.println(spdB);
+  Serial.println(spdC);
+  Serial.println();
+  Serial.println();
+}
+void loop()
+{
+  delay(1000);
+  turnoff();
+  beddermoveohmi(255, 0);
+  delay(1000);
+  turnoff();
+  Amove(255);
+  delay(1000);
+  turnoff();
+  Bmove(255);
+  delay(1000);
+  turnoff();
+  Cmove(255);
+  delay(1000);
+  turnoff();
+  turn(254);
+  delay(1000);
+}
diff --git a/debug/tcs34725/tcs34725.ino b/debug/tcs34725/tcs34725.ino
new file mode 100644
index 0000000..88d1467
--- /dev/null
+++ b/debug/tcs34725/tcs34725.ino
@@ -0,0 +1,45 @@
+#include <Wire.h>
+#include "Adafruit_TCS34725.h"
+
+/* Example code for the Adafruit TCS34725 breakout library */
+
+/* Connect SCL    to analog 5
+   Connect SDA    to analog 4
+   Connect VDD    to 3.3V DC
+   Connect GROUND to common ground */
+
+/* Initialise with default values (int time = 2.4ms, gain = 1x) */
+// Adafruit_TCS34725 tcs = Adafruit_TCS34725();
+
+/* Initialise with specific int time and gain values */
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_614MS, TCS34725_GAIN_1X);
+
+void setup(void) {
+  Serial.begin(9600);
+
+  if (tcs.begin()) {
+    Serial.println("Found sensor");
+  } else {
+    Serial.println("No TCS34725 found ... check your connections");
+    while (1);
+  }
+
+  // Now we're ready to get readings!
+}
+
+void loop(void) {
+  uint16_t r, g, b, c, colorTemp, lux;
+
+  tcs.getRawData(&r, &g, &b, &c);
+  // colorTemp = tcs.calculateColorTemperature(r, g, b);
+  colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);
+  lux = tcs.calculateLux(r, g, b);
+
+  Serial.print("Color Temp: "); Serial.print(colorTemp, DEC); Serial.print(" K - ");
+  Serial.print("Lux: "); Serial.print(lux, DEC); Serial.print(" - ");
+  Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
+  Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
+  Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
+  Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");
+  Serial.println(" ");
+}