Motor Calibration capabilities

11 files changed, 586 insertions, 8 deletions

diff --git a/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU-9-test.py b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU-9-test.py
index 0459874..993c8e8 100644
--- a/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU-9-test.py
+++ b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU-9-test.py
@@ -36,7 +36,7 @@
 # Install pyserial from http://sourceforge.net/projects/pyserial/files/
 # Install VPython from http://vpython.org/contents/download_windows.html
 
-from vpython import *
+from visual import *
 import serial
 import string
 import math
diff --git a/debug/data_logging_teensy_temp/InternalTemperature-master.zip b/debug/data_logging_teensy_temp/InternalTemperature-master.zip
new file mode 100644
index 0000000..64b7dc4
--- /dev/null
+++ b/debug/data_logging_teensy_temp/InternalTemperature-master.zip
diff --git a/debug/data_logging_teensy_temp/data_logging_teensy_temp.ino b/debug/data_logging_teensy_temp/data_logging_teensy_temp.ino
new file mode 100644
index 0000000..fcd6c64
--- /dev/null
+++ b/debug/data_logging_teensy_temp/data_logging_teensy_temp.ino
@@ -0,0 +1,63 @@
+#include <InternalTemperature.h>
+#include <SD.h>
+#include <SPI.h>
+
+File ;
+File avrage;
+const int chipSelect = BUILTIN_SDCARD;
+
+/
+
+void setup()
+{
+  Serial.begin(9600);
+  
+  //open current loop motor values file
+  current = SD.open("motor_values.csv", FILE_WRITE);
+  
+  while(true){
+    if (current) {
+      if (SD.exists((String("motor_values") + x + ".csv").c_str())){
+        Serial.println(((String)"motor_values"+x+".csv already exists"));
+        x++;
+      } else {
+        Serial.println((String)"motor_values"+x+".csv"" doesn't exist.");
+        Serial.println((String)"creating motor_values"+x+".csv");
+        current = SD.open(((String("motor_values") + x + ".csv").c_str()), FILE_WRITE);
+        break;
+      }
+    }
+    delay(1);
+  }
+  Serial.println("!!!setup complete!!!");
+}
+
+
+void loop()
+{
+  String currentvals = "";
+  String avrvals = "";
+  
+  currentvals += InternalTemperature.readTemperatureC();
+  d_println("°C");
+  debugdataString += "°C";
+  
+  //log sensor data to sensor log
+  sensordataString += InternalTemperature.readTemperatureC();
+  sensordataString += ", ";
+
+  //open sensor log file 
+  File csvfile = SD.open((String("motor_values") + x + ".csv").c_str()), FILE_WRITE);
+  if (csvfile) {
+    csvfile.println(sensordataString);
+    csvfile.close();
+  }
+
+  //open debug file
+  File dataFile = SD.open("avrage_motor_values.txt", FILE_WRITE);
+  if (dataFile) {
+    dataFile.println((String)"debug data:"+debugdataString+"sensor data"+sensordataString);
+    dataFile.close();
+  }
+  delay(1000);
+}
diff --git a/debug/motordriver/PID/PID.ino b/debug/motordriver/PID/PID.ino
new file mode 100644
index 0000000..313b440
--- /dev/null
+++ b/debug/motordriver/PID/PID.ino
@@ -0,0 +1,116 @@
+#include <TimerThree.h>
+#include "ArduPID.h"
+
+ArduPID myController;
+
+double input;
+double output;
+double setpoint = 950; // Desired RPM
+double p = 40; // PID P-term
+double i = 2;  // PID I-term
+double d = 0; // PID D-term
+
+int enA = 1;  // PWM pin
+int enB = 2;  // GND
+int pwm1 = 3; // Motor control pin 1
+int pwm2 = 4; // Motor control pin 2
+int encoA = 5; // Encoder pin A
+int encoB = 6; // Encoder pin B
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-1000
+
+void setup() {
+  pinMode(enA, OUTPUT);
+  pinMode(enB, OUTPUT);
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+
+  pinMode(encoA, INPUT_PULLUP);
+  pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
+
+  // PID settings
+  myController.begin(&input, &output, &setpoint, p, i, d);
+  myController.setOutputLimits(0, 1000); // Output limits for PWM control
+  myController.setBias(500); // Starting bias for PWM
+  myController.setWindUpLimits(-50, 50);
+
+  myController.start();
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(enA, HIGH);
+  } else {
+    digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 1000) {
+    counter = 0;
+  }
+}
+
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 1000) value = 1000;
+
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime); // Adjust the encoder counts per revolution here if necessary
+  encount = 0;
+}
+
+void resetenc() {
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped) * 0.1;
+  mcpsoftpwm(sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  }
+}
+
+void loop() {
+  readenc();
+  input = rpm;
+  myController.compute();
+  myController.debug(&Serial, "myController", PRINT_INPUT);
+  
+  move(output); // Replace with plant control signal
+  delay(100);
+}
diff --git a/debug/motordriver/calibrated_motor/calibrated_motor.ino b/debug/motordriver/calibrated_motor/calibrated_motor.ino
new file mode 100644
index 0000000..9367f2a
--- /dev/null
+++ b/debug/motordriver/calibrated_motor/calibrated_motor.ino
@@ -0,0 +1,153 @@
+#include <TimerThree.h>
+#include <SD.h>
+#include <SPI.h>
+
+#define ArrayElementCount(needRPM) (sizeof(needRPM) / sizeof(needRPM[0]))
+
+int enA = 1;  //PWM pin
+int enB = 2;  //GND
+int pwm1 = 3; //
+int pwm2 = 4;
+int encoA = 5;
+int encoB = 6;
+
+const int maxspeed = 1000;
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-100
+
+int motordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910};
+int needRPM = 41;
+
+
+void setup() {
+  pinMode(enA, OUTPUT);
+  pinMode(enB, OUTPUT);
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+  
+
+  pinMode(encoA, INPUT_PULLUP);
+  pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(enA, HIGH);
+  } else {
+    digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 100) value = 100;
+
+  //Serial.print("softPWM pin: enA, value: ");
+  //Serial.println(value); 
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime);
+  //Serial.print("RPM: ");
+  //Serial.print(rpm);
+  //Serial.print("    Sample period:");
+  //Serial.print(Ptime);
+  //Serial.print("ms, encoder count:");
+  //Serial.println(encount);
+
+  encount = 0;
+}
+void resetenc(){
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped)*0.1;
+  mcpsoftpwm(sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    //Serial.print("back ");
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    //Serial.print("forward ");
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  } 
+  //Serial.println(sp);
+}
+
+
+void loop() {
+  for (int i = 0; i < maxspeed; i++) {
+    Serial.print(motordata[i]);
+    Serial.print(", ");
+  }
+  Serial.println("");
+  int idx = nearest(needRPM, motordata, ArrayElementCount(motordata));
+  Serial.print(needRPM);
+  Serial.print(", ");
+  Serial.print(idx);
+  Serial.print(", ");
+  Serial.println(motordata[idx]);
+  delay(2500);
+  move(idx);
+  delay(1000);
+  resetenc();
+  delay(100);
+  readenc();
+  Serial.println(rpm);
+  delay(10);
+
+
+  
+}
+int nearest(int x, int myArray[], int elements)
+{
+  //https://forum.arduino.cc/t/search-for-nearest-number-in-an-array-help/245843/5
+  int idx = 0; // by default near first element
+
+  int distance = abs(myArray[idx] - x);
+  for (int i = 1; i < elements; i++)
+  {
+    int d = abs(myArray[i] - x);
+    if (d < distance)
+    {
+      idx = i;
+      distance = d;
+    }
+  }
+  return idx;
+}
diff --git a/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp b/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp
new file mode 100644
index 0000000..3b62227
--- /dev/null
+++ b/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp
diff --git a/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat b/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat
new file mode 100644
index 0000000..dbbfee0
--- /dev/null
+++ b/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat
@@ -0,0 +1 @@
+0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, 287, 288, 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312, 313, 314, 315, 316, 317, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 330, 331, 332, 333, 334, 335, 336, 337, 338, 339, 340, 341, 342, 343, 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 355, 356, 357, 358, 359, 360, 361, 362, 363, 364, 365, 366, 367, 368, 369, 370, 371, 372, 373, 374, 375, 376, 377, 378, 379, 380, 381, 382, 383, 384, 385, 386, 387, 388, 389, 390, 391, 392, 393, 394, 395, 396, 397, 398, 399, 400, 401, 402, 403, 404, 405, 406, 407, 408, 409, 410, 411, 412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 424, 425, 426, 427, 428, 429, 430, 431, 432, 433, 434, 435, 436, 437, 438, 439, 440, 441, 442, 443, 444, 445, 446, 447, 448, 449, 450, 451, 452, 453, 454, 455, 456, 457, 458, 459, 460, 461, 462, 463, 464, 465, 466, 467, 468, 469, 470, 471, 472, 473, 474, 475, 476, 477, 478, 479, 480, 481, 482, 483, 484, 485, 486, 487, 488, 489, 490, 491, 492, 493, 494, 495, 496, 497, 498, 499, 500, 501, 502, 503, 504, 505, 506, 507, 508, 509, 510, 511, 512, 513, 514, 515, 516, 517, 518, 519, 520, 521, 522, 523, 524, 525, 526, 527, 528, 529, 530, 531, 532, 533, 534, 535, 536, 537, 538, 539, 540, 541, 542, 543, 544, 545, 546, 547, 548, 549, 550, 551, 552, 553, 554, 555, 556, 557, 558, 559, 560, 561, 562, 563, 564, 565, 566, 567, 568, 569, 570, 571, 572, 573, 574, 575, 576, 577, 578, 579, 580, 581, 582, 583, 584, 585, 586, 587, 588, 589, 590, 591, 592, 593, 594, 595, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609, 610, 611, 612, 613, 614, 615, 616, 617, 618, 619, 620, 621, 622, 623, 624, 625, 626, 627, 628, 629, 630, 631, 632, 633, 634, 635, 636, 637, 638, 639, 640, 641, 642, 643, 644, 645, 646, 647, 648, 649, 650, 651, 652, 653, 654, 655, 656, 657, 658, 659, 660, 661, 662, 663, 664, 665, 666, 667, 668, 669, 670, 671, 672, 673, 674, 675, 676, 677, 678, 679, 680, 681, 682, 683, 684, 685, 686, 687, 688, 689, 690, 691, 692, 693, 694, 695, 696, 697, 698, 699, 700, 701, 702, 703, 704, 705, 706, 707, 708, 709, 710, 711, 712, 713, 714, 715, 716, 717, 718, 719, 720, 721, 722, 723, 724, 725, 726, 727, 728, 729, 730, 731, 732, 733, 734, 735, 736, 737, 738, 739, 740, 741, 742, 743, 744, 745, 746, 747, 748, 749, 750, 751, 752, 753, 754, 755, 756, 757, 758, 759, 760, 761, 762, 763, 764, 765, 766, 767, 768, 769, 770, 771, 772, 773, 774, 775, 776, 777, 778, 779, 780, 781, 782, 783, 784, 785, 786, 787, 788, 789, 790, 791, 792, 793, 794, 795, 796, 797, 798, 799, 800, 801, 802, 803, 804, 805, 806, 807, 808, 809, 810, 811, 812, 813, 814, 815, 816, 817, 818, 819, 820, 821, 822, 823, 824, 825, 826, 827, 828, 829, 830, 831, 832, 833, 834, 835, 836, 837, 838, 839, 840, 841, 842, 843, 844, 845, 846, 847, 848, 849, 850, 851, 852, 853, 854, 855, 856, 857, 858, 859, 860, 861, 862, 863, 864, 865, 866, 867, 868, 869, 870, 871, 872, 873, 874, 875, 876, 877, 878, 879, 880, 881, 882, 883, 884, 885, 886, 887, 888, 889, 890, 891, 892, 893, 894, 895, 896, 897, 898, 899, 900, 901, 902, 903, 904, 905, 906, 907, 908, 909, 910, 911, 912, 913, 914, 915, 916, 917, 918, 919, 920, 921, 922, 923, 924, 925, 926, 927, 928, 929, 930, 931, 932, 933, 934, 935, 936, 937, 938, 939, 940, 941, 942, 943, 944, 945, 946, 947, 948, 949, 950, 951, 952, 953, 954, 955, 956, 957, 958, 959, 960, 961, 962, 963, 964, 965, 966, 967, 968, 969, 970, 971, 972, 973, 974, 975, 976, 977, 978, 979, 980, 981, 982, 983, 984, 985, 986, 987, 988, 989, 990, 991, 992, 993, 994, 995, 996, 997, 998, 999, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910, 
\ No newline at end of file
diff --git a/debug/motordriver/motor_calibration/motor_calibration.ino b/debug/motordriver/motor_calibration/motor_calibration.ino
new file mode 100644
index 0000000..362d130
--- /dev/null
+++ b/debug/motordriver/motor_calibration/motor_calibration.ino
@@ -0,0 +1,231 @@
+#include <TimerThree.h>
+#include <SD.h>
+#include <SPI.h>
+
+int enA = 1;  //PWM pin
+int enB = 2;  //GND
+int pwm1 = 3; //
+int pwm2 = 4;
+int encoA = 5;
+int encoB = 6;
+
+const int maxspeed = 1000;
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-100
+File ;
+File avrage;
+Sd2Card card;
+SdVolume volume;
+SdFile root;
+const int chipSelect = BUILTIN_SDCARD;
+int x = 1;
+
+int Nloop = 0;
+int totloopdata[maxspeed] = {};
+
+
+void setup() {
+  pinMode(enA, OUTPUT);
+  pinMode(enB, OUTPUT);
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+
+  pinMode(encoA, INPUT_PULLUP);
+  pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(enA, HIGH);
+  } else {
+    digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 100) value = 100;
+
+  //Serial.print("softPWM pin: enA, value: ");
+  //Serial.println(value); 
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime);
+  //Serial.print("RPM: ");
+  //Serial.print(rpm);
+  //Serial.print("    Sample period:");
+  //Serial.print(Ptime);
+  //Serial.print("ms, encoder count:");
+  //Serial.println(encount);
+
+  encount = 0;
+}
+void resetenc(){
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped)*0.1;
+  mcpsoftpwm(sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    //Serial.print("back ");
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    //Serial.print("forward ");
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  } 
+  //Serial.println(sp);
+}
+
+void loop() {
+  move(0);
+  //max
+  /*
+  Serial.println();
+  Serial.println();
+  Serial.println();
+  move(maxspeed);
+  Serial.print("max speed: ");
+  delay(1000);
+  resetenc();
+  delay(1000);
+  readenc();
+  Serial.print(rpm);
+  Serial.print("rpm, with input: ");
+  Serial.println(maxspeed);
+  delay(250);
+  move(0);
+
+  //min
+  delay(5000);
+  Serial.print("min speed: ");
+  for (int i = 0; i < maxspeed; i++) {
+    move(i);  
+    delay(1000);
+    readenc();
+    if ((rpm > 10) && (i>5)) {
+      Serial.print(rpm);
+      Serial.print("rpm, with input: ");
+      Serial.println(i);
+      break; 
+    }
+  }
+  */
+  delay(2000);
+  for (int i = 0; i < maxspeed; i++) {
+    Serial.println("0,0");
+  }
+  int currentloopdata[maxspeed] = {};
+  for (int i = 0; i < maxspeed; i++) {
+    move(i);  
+    delay(100);
+    readenc();
+    Serial.print(i);
+    Serial.print(",");
+    Serial.print(rpm);
+    currentloopdata[i] = rpm;
+    Serial.print(",");
+    Serial.println(totloopdata[i]);
+  }
+  /*
+  for (int i = 0; i < maxspeed; i++) {
+    move(0); 
+    delay(200); 
+    move(i); 
+    delay(1000); 
+    resetenc();
+    delay(100); 
+    readenc();
+    Serial.print(i);
+    Serial.print(",");
+    Serial.print(rpm);
+    currentloopdata[i] = rpm;
+    Serial.print(",");
+    Serial.println(totloopdata[i]);
+  }
+  */
+  currentloopdata[0] = 0;
+  if (Nloop == 0){
+    for (int i = 0; i < maxspeed; i++) {
+      totloopdata[i] = currentloopdata[i];
+    }
+  }else{
+    for (int i = 0; i < maxspeed; i++) {
+      //Serial.print(currentloopdata[i]);
+      //Serial.print(",  ");
+      //Serial.print(totloopdata[i]);
+      totloopdata[i] = ((totloopdata[i]*Nloop)+currentloopdata[i])/(Nloop+1);
+    }
+  }
+  for (int i = 0; i < maxspeed; i++) {
+    //Serial.print(currentloopdata[i]);
+    //Serial.print(",  ");
+  }
+
+  resetenc();
+  SD.remove("avrage_motor_values.dat");
+  File dataFile = SD.open("avrage_motor_values.dat", FILE_WRITE);
+  if (dataFile) {
+    for (int i = 0; i < maxspeed; i++) {
+      dataFile.print(totloopdata[i]);
+      dataFile.print(", ");
+  }
+    dataFile.close();
+  }
+  SD.remove("avrage_motor_values.csv");
+  File debugFile = SD.open("avrage_motor_values.csv", FILE_WRITE);
+  if (debugFile) {
+    for (int i = 0; i < maxspeed; i++) {
+      debugFile.print(i);
+      debugFile.print(", ");
+      debugFile.println(totloopdata[i]);
+    }
+  }
+
+  if (!card.init(SPI_HALF_SPEED, chipSelect)) {
+    Serial.println("FAILED TO FIND SD CARD");
+    Serial.println("* is a card inserted?");
+    Serial.println("* is your wiring correct?");
+    Serial.println("* did you change the chipSelect pin to match your shield or module?");
+  }
+  
+  
+  Nloop++;
+  
+  
+}
diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
index 2b159f2..b92d976 100644
--- a/debug/motordriver/motordriver.ino
+++ b/debug/motordriver/motordriver.ino
@@ -5,7 +5,8 @@
 //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
-//
+//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf
+
 // motor one/A  front
 #define  m1enA 3   // GPA3  PWM1
 #define  m1enB 2   // GPA2
diff --git a/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino b/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino
index b9335f3..2f57d59 100644
--- a/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino
+++ b/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino
@@ -7,7 +7,7 @@ int pwm2 = 4;
 int encoA = 5;
 int encoB = 6;
 
-int speed = 1; // speed 0-1
+int speed = 1000; // speed 0-1
 
 double sp = 0.00;
 double rpm = 0.00;
@@ -85,7 +85,7 @@ void resetenc(){
 }
 
 void move(int sped) {
-  sp = abs(sped)*100;
+  sp = abs(sped)*0.1;
   mcpsoftpwm(sp);
   digitalWrite(enB, LOW);
   if (sped < 0) {
@@ -104,14 +104,23 @@ void loop() {
   move(speed);
   delay(1000);
   readenc();
+  delay(1000);
+
+  Serial.println();
 
-  
   move(0);
+  delay(1000);
   resetenc();
   delay(1000);
   readenc();
+  delay(1000);
 
-  move(-speed);
+  Serial.println();
+
+  move(500);
   delay(1000);
   readenc();
+  delay(1000);
+
+  Serial.println();
 }
diff --git a/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino b/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino
index 37128ce..5c8ff2a 100644
--- a/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino
+++ b/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino
@@ -1,6 +1,10 @@
 #include <TimerThree.h>
 #include <Adafruit_MCP23X17.h>
 
+//https://www.pololu.com/product/2997
+//https://www.pololu.com/product/4861
+//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf
+
 Adafruit_MCP23X17 mcp;
 
 int enA = 0;    // GPA0 -PWM
@@ -10,7 +14,7 @@ int pwm2 = 3;   // GPA3 -HIGH/LOW
 int encoA = 4;  // GPA4 -read
 int encoB = 5;  // GPA5 -read
 
-int speed = 1; // speed 0-1
+int speed = 100; // speed 0-1
 
 double sp = 0.00;
 double rpm = 0.00;
@@ -92,7 +96,7 @@ void resetenc(){
 }
 
 void move(int sped) {
-  sp = abs(sped)*100;
+  sp = abs(sped)*1;
   mcpsoftpwm(sp);
   mcp.digitalWrite(enB, LOW);
   if (sped < 0) {