to fix: change mcp2.analogWrite(m1pwm1, spdA); to us softPWM

9 files changed, 732 insertions, 218 deletions

diff --git a/debug/motordriver/arduino-softpwm-master.zip b/debug/motordriver/arduino-softpwm-master.zip
new file mode 100644
index 0000000..dc713b7
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master.zip
diff --git a/debug/motordriver/arduino-softpwm-master/.gitignore b/debug/motordriver/arduino-softpwm-master/.gitignore
new file mode 100644
index 0000000..1605aab
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/.gitignore
@@ -0,0 +1 @@
+SoftPWM.h.gch
\ No newline at end of file
diff --git a/debug/motordriver/arduino-softpwm-master/README.md b/debug/motordriver/arduino-softpwm-master/README.md
new file mode 100644
index 0000000..46e5456
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/README.md
@@ -0,0 +1,75 @@
+arduino-softpwm
+===============
+
+Software PWM library for [Arduino](https://arduino.cc/), and other compatible AVR boards.
+
+AVR microcontrollers provide hardware PWM on some pins but if you need PWM on other pins then it must be implemented in software. This library provides easy and efficient software PWM on any pin. Each channel can be set to a different PWM duty cycle.
+
+This library is licensed under [BSD3](https://opensource.org/licenses/BSD-3-Clause)
+
+#### Installation
+- Download the most recent version here: https://github.com/Palatis/arduino-softpwm/archive/master.zip
+- Using Arduino IDE 1.0.x:
+  - Sketch > Import Library... > Add Library... > select the downloaded file > Open
+- Using Arduino IDE 1.5+:
+  - Sketch > Include Library > Add ZIP Library... > select the downloaded file > Open
+
+
+#### Usage
+See **File > Examples > arduino-softpwm > SoftPWM_example** for demonstration of library usage.
+
+Everything in this library except for preprocessor defines are wrapped in `namespace Palatis {}`, so if your compiler blames you about symbol not found, try add `Palatis::` before that thing. For example, `Palatis::SoftPWM.size()`.
+
+Alternatively, you can add `using namespace Palatis;` at the top of the source file to import everything under the `Palatis` namespace, this might be convenient if you're using my other libraries.
+
+`#define SOFTPWM_OUTPUT_DELAY` - Add this line above `#include <SoftPWM.h>` for a 1 PWM clock cycle delay between outputs to prevent large in-rush currents.
+
+`SOFTPWM_DEFINE_CHANNEL(CHANNEL, PMODE, PORT, BIT)` - Configure a pin for software PWM use. Consult the datasheet for your microcontroller for the appropriate `PORT` and `BIT` values for the physical pin. This information is shown in the pinout diagram, for example: [ATmega328P datasheet](http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Summary.pdf) **Figure 1-1** found on page 3. If you want to determine the Arduino pin assigned to the physical pin http://www.pighixxx.com/test/pinoutspg/boards/ provides this information for the most popular Arduino boards or you can look at the pins_arduino.h file in the **variant** folder used by your board.
+- Parameter: **CHANNEL** - The channel number is used as an ID for the pin.
+- Parameter: **PMODE** - DDRx register of the pin's port. Append the port letter to `DDR`. For example: The [Arduino UNO diagram](http://www.pighixxx.com/test/portfolio-items/uno/?portfolioID=314) shows that Arduino pin 13 is **PB5** which means that the port is **B** so you should use the value `DDRB` for that pin.
+- Parameter: **PORT** - The port of the pin. For example: Arduino pin 13 is **PB5** so you should use the value `PORTB` for that pin.
+- Parameter: **BIT** - The bit of the pin. For example: Arduino pin 13 is **PB5** so you should use the value `PORTB5` for that pin.
+
+`SOFTPWM_DEFINE_CHANNEL_INVERT( CHANNEL, PMODE, PORT, BIT )` - Depending on your application you may prefer to invert the output. See `SOFTPWM_DEFINE_CHANNEL()` for description of parameters.
+
+`SOFTPWM_DEFINE_OBJECT(CHANNEL_CNT)` - Define the softPWM object with the default 256 PWM levels.
+- Parameter: **CHANNEL_CNT** - The number of channels that are defined.
+
+`SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS)` - Define the softPWM object with the specified number of PWM levels.
+- Parameter: **CHANNEL_CNT** - The number of channels that are defined.
+- Parameter: **PWM_LEVELS** - The number of PWM levels. Using less PWM levels may allow a higher PWM frequency. The maximum value is 256.
+
+`SOFTPWM_DEFINE_EXTERN_OBJECT(CHANNEL_CNT)` - Add this if you want to use the SoftPWM object outside where it's defined. See `SOFTPWM_DEFINE_OBJECT()` for description of the parameter.
+
+`SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS)` - Add this if you want to use the SoftPWM object outside where it's defined. See `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` for description of parameters.
+
+`SoftPWM.begin(hertz)` - Initialize SoftPWM.
+- Parameter: **hertz** - The PWM frequency. Setting the value too high will cause incorrect operation, too low will cause a visible flicker.
+  - Type: long
+
+`SoftPWM.printInterruptLoad()` - Prints diagnostic information to the serial monitor. This can be used to find the optimal PWM frequency by setting different PWM frequency values in begin() and then checking the resulting interrupt load. Calling this function will momentarily turn off the PWM on all channels.
+
+`SoftPWM.set(channel_idx, value)` - Set the PWM level of the given channel.
+- Parameter: **channel_idx** - The channel to set.
+  - Type: int
+- Parameter: **value** - The PWM level to set.
+  - Type: byte
+
+`SoftPWM.size()`
+- Returns: Number of channels defined.
+  - Type: size_t
+
+`SoftPWM.PWMlevels()`
+- Returns: The number of PWM levels.
+  - Type: unsigned int
+
+`SoftPWM.allOff()` - Set the PWM value of all channels to 0.
+
+
+#### Troubleshooting
+- LEDs have a visible flicker, especially noticeable when the LED is moving relative to the viewer.
+  - The PWM frequency set in `SoftPWM.begin()` is too low. Use `SoftPWM.printInterruptLoad()` to determine the optimum PWM frequency. You may be able to achieve a higher PWM frequency by setting less PWM levels with `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` or `SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS()`.
+- Erratic PWM operation
+  - The interrupt load is too high. Use `SoftPWM.printInterruptLoad()` to determine the interrupt load. You can decrease the interrupt load by setting less PWM levels with `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` or `SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS()` or setting the PWM frequency lower in `SoftPWM.begin()`.
+- LED brightness changes between low brightness PWM values are larger than at brighter PWM values.
+  - This is caused by the way LEDs work and is not caused by a problem with the library. If possible, use more PWM levels or never allow the LED to get dimmer than the level below which the difference between PWM levels is too distinct.
diff --git a/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino b/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino
new file mode 100644
index 0000000..96fc0b2
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino
@@ -0,0 +1,141 @@
+/*
+  Arduino-SoftPWM: a software PWM library for Arduino
+  Copyright 2016, Victor Tseng <palatis@gmail.com>
+
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+  1. Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+  2. Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+  3. Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <SoftPWM.h>
+
+/* pins_arduino.h defines the pin-port/bit mapping as PROGMEM so
+   you have to read them with pgm_read_xxx(). That's generally okay
+   for ordinary use, but really bad when you're writing super fast
+   codes because the compiler doesn't treat them as constants and
+   cannot optimize them away with sbi/cbi instructions.
+
+   Therefore we have to tell the compiler the PORT and BIT here.
+   Hope someday we can find a way to workaround this.
+
+   Check the manual of your MCU for port/bit mapping.
+
+   The following example demonstrates setting channels for all pins
+   on the ATmega328P or ATmega168 used on Arduino Uno, Pro Mini,
+   Nano and other boards. */
+SOFTPWM_DEFINE_CHANNEL(0, DDRD, PORTD, PORTD0);  //Arduino pin 0
+SOFTPWM_DEFINE_CHANNEL(1, DDRD, PORTD, PORTD1);  //Arduino pin 1
+SOFTPWM_DEFINE_CHANNEL(2, DDRD, PORTD, PORTD2);  //Arduino pin 2
+SOFTPWM_DEFINE_CHANNEL(3, DDRD, PORTD, PORTD3);  //Arduino pin 3
+SOFTPWM_DEFINE_CHANNEL(4, DDRD, PORTD, PORTD4);  //Arduino pin 4
+SOFTPWM_DEFINE_CHANNEL(5, DDRD, PORTD, PORTD5);  //Arduino pin 5
+SOFTPWM_DEFINE_CHANNEL(6, DDRD, PORTD, PORTD6);  //Arduino pin 6
+SOFTPWM_DEFINE_CHANNEL(7, DDRD, PORTD, PORTD7);  //Arduino pin 7
+SOFTPWM_DEFINE_CHANNEL(8, DDRB, PORTB, PORTB0);  //Arduino pin 8
+SOFTPWM_DEFINE_CHANNEL(9, DDRB, PORTB, PORTB1);  //Arduino pin 9
+SOFTPWM_DEFINE_CHANNEL(10, DDRB, PORTB, PORTB2);  //Arduino pin 10
+SOFTPWM_DEFINE_CHANNEL(11, DDRB, PORTB, PORTB3);  //Arduino pin 11
+SOFTPWM_DEFINE_CHANNEL(12, DDRB, PORTB, PORTB4);  //Arduino pin 12
+SOFTPWM_DEFINE_CHANNEL(13, DDRB, PORTB, PORTB5);  //Arduino pin 13
+SOFTPWM_DEFINE_CHANNEL(14, DDRC, PORTC, PORTC0);  //Arduino pin A0
+SOFTPWM_DEFINE_CHANNEL(15, DDRC, PORTC, PORTC1);  //Arduino pin A1
+SOFTPWM_DEFINE_CHANNEL(16, DDRC, PORTC, PORTC2);  //Arduino pin A2
+SOFTPWM_DEFINE_CHANNEL(17, DDRC, PORTC, PORTC3);  //Arduino pin A3
+SOFTPWM_DEFINE_CHANNEL(18, DDRC, PORTC, PORTC4);  //Arduino pin A4
+SOFTPWM_DEFINE_CHANNEL(19, DDRC, PORTC, PORTC5);  //Arduino pin A5
+
+
+/* Or you may want inverted outputs: */
+/*
+  SOFTPWM_DEFINE_CHANNEL_INVERT(0, DDRD, PORTD, PORTD0);  //Arduino pin 0
+  SOFTPWM_DEFINE_CHANNEL_INVERT(1, DDRD, PORTD, PORTD1);  //Arduino pin 1
+  SOFTPWM_DEFINE_CHANNEL_INVERT(2, DDRD, PORTD, PORTD2);  //Arduino pin 2
+  SOFTPWM_DEFINE_CHANNEL_INVERT(3, DDRD, PORTD, PORTD3);  //Arduino pin 3
+  SOFTPWM_DEFINE_CHANNEL_INVERT(4, DDRD, PORTD, PORTD4);  //Arduino pin 4
+  SOFTPWM_DEFINE_CHANNEL_INVERT(5, DDRD, PORTD, PORTD5);  //Arduino pin 5
+  SOFTPWM_DEFINE_CHANNEL_INVERT(6, DDRD, PORTD, PORTD6);  //Arduino pin 6
+  SOFTPWM_DEFINE_CHANNEL_INVERT(7, DDRD, PORTD, PORTD7);  //Arduino pin 7
+  SOFTPWM_DEFINE_CHANNEL_INVERT(8, DDRB, PORTB, PORTB0);  //Arduino pin 8
+  SOFTPWM_DEFINE_CHANNEL_INVERT(9, DDRB, PORTB, PORTB2);  //Arduino pin 9
+  SOFTPWM_DEFINE_CHANNEL_INVERT(10, DDRB, PORTB, PORTB2);  //Arduino pin 10
+  SOFTPWM_DEFINE_CHANNEL_INVERT(11, DDRB, PORTB, PORTB3);  //Arduino pin 11
+  SOFTPWM_DEFINE_CHANNEL_INVERT(12, DDRB, PORTB, PORTB4);  //Arduino pin 12
+  SOFTPWM_DEFINE_CHANNEL_INVERT(13, DDRB, PORTB, PORTB5);  //Arduino pin 13
+  SOFTPWM_DEFINE_CHANNEL_INVERT(14, DDRC, PORTC, PORTC0);  //Arduino pin A0
+  SOFTPWM_DEFINE_CHANNEL_INVERT(15, DDRC, PORTC, PORTC1);  //Arduino pin A1
+  SOFTPWM_DEFINE_CHANNEL_INVERT(16, DDRC, PORTC, PORTC2);  //Arduino pin A2
+  SOFTPWM_DEFINE_CHANNEL_INVERT(17, DDRC, PORTC, PORTC3);  //Arduino pin A3
+  SOFTPWM_DEFINE_CHANNEL_INVERT(18, DDRC, PORTC, PORTC4);  //Arduino pin A4
+  SOFTPWM_DEFINE_CHANNEL_INVERT(19, DDRC, PORTC, PORTC5);  //Arduino pin A5
+*/
+
+/* Here you make an instance of desired channel counts you want
+   with the default 256 PWM levels (0 ~ 255). */
+//SOFTPWM_DEFINE_OBJECT(20);
+
+/* Or you can make one with only 100 PWM levels (0 ~ 99).
+   By using less PWM levels, you may be able to use higher
+   pwm frequencies. */
+SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(20, 100);
+
+/* If you want to use the SoftPWM object outside where it's defined,
+   add the following line to the file. */
+//SOFTPWM_DEFINE_EXTERN_OBJECT(16);
+SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(20, 100);
+
+void setup() {
+  Serial.begin(19200);
+
+  // begin with 60hz pwm frequency
+  Palatis::SoftPWM.begin(60);
+
+  // print interrupt load for diagnostic purposes
+  Palatis::SoftPWM.printInterruptLoad();
+}
+
+static volatile uint8_t v = 0;
+void loop() {
+  for (uint8_t i = 0; i < Palatis::SoftPWM.size(); ++i) {
+    Serial.print(micros());
+    Serial.print(" loop(): ");
+    Serial.println(i);
+
+    unsigned long const WAIT = 1000000 / Palatis::SoftPWM.PWMlevels() / 2;
+    unsigned long nextMicros = 0;
+    for (int v = 0; v < Palatis::SoftPWM.PWMlevels() - 1; ++v) {
+      while (micros() < nextMicros);
+      nextMicros = micros() + WAIT;
+      Palatis::SoftPWM.set(i, v);
+    }
+    for (int v = Palatis::SoftPWM.PWMlevels() - 1; v >= 0; --v) {
+      while (micros() < nextMicros);
+      nextMicros = micros() + WAIT;
+      Palatis::SoftPWM.set(i, v);
+    }
+  }
+}
diff --git a/debug/motordriver/arduino-softpwm-master/keywords.txt b/debug/motordriver/arduino-softpwm-master/keywords.txt
new file mode 100644
index 0000000..02bbe68
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/keywords.txt
@@ -0,0 +1,30 @@
+#######################################

+# Syntax Coloring Map SoftPWM

+#######################################

+#######################################

+# Datatypes (KEYWORD1)

+#######################################

+Palatis	KEYWORD1

+SoftPWM	KEYWORD1

+#######################################

+# Methods and Functions (KEYWORD2)

+#######################################

+begin	KEYWORD2

+set	KEYWORD2

+size	KEYWORD2

+PWMlevels	KEYWORD2

+allOff	KEYWORD2

+update	KEYWORD2

+printInterruptLoad	KEYWORD2

+

+#######################################

+# Constants (LITERAL1)

+#######################################

+SOFTPWM_DEFINE_PINMODE	LITERAL1

+SOFTPWM_DEFINE_CHANNEL	LITERAL1

+SOFTPWM_DEFINE_CHANNEL_INVERT	LITERAL1

+SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS	LITERAL1

+SOFTPWM_DEFINE_OBJECT	LITERAL1

+SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS	LITERAL1

+SOFTPWM_DEFINE_EXTERN_OBJECT	LITERAL1

+SOFTPWM_OUTPUT_DELAY	LITERAL1

diff --git a/debug/motordriver/arduino-softpwm-master/library.properties b/debug/motordriver/arduino-softpwm-master/library.properties
new file mode 100644
index 0000000..174c19b
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/library.properties
@@ -0,0 +1,9 @@
+name=arduino-softpwm
+version=1.0.1
+author=Victor Tseng <palatis@gmail.com>
+maintainer=Victor Tseng <palatis@gmail.com>
+sentence=Software PWM library for Arduino.
+paragraph=Provides PWM on any pin.
+category=Signal Input/Output
+url=https://github.com/Palatis/arduino-softpwm
+architectures=avr
diff --git a/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h b/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h
new file mode 100644
index 0000000..c315a4e
--- /dev/null
+++ b/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h
@@ -0,0 +1,255 @@
+/*
+  Arduino-SoftPWM: a software PWM library for Arduino
+  Copyright 2016, Victor Tseng <palatis@gmail.com>
+
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+  1. Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+  2. Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+  3. Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef _SOFTPWM_H_
+#define _SOFTPWM_H_
+
+#include <Arduino.h>
+
+// helper macros
+#define SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT) \
+  namespace Palatis { \
+    template <> \
+    inline void pinModeStatic<CHANNEL>(uint8_t const mode) { \
+      if (mode == INPUT) { \
+        bitClear(PMODE, BIT); bitClear(PORT, BIT); \
+      } \
+      else if (mode == INPUT_PULLUP) { \
+        bitClear(PMODE, BIT); bitSet(PORT, BIT); \
+      } \
+      else { \
+        bitSet(PMODE, BIT); \
+      } \
+    } \
+  }
+
+#define SOFTPWM_DEFINE_CHANNEL(CHANNEL, PMODE, PORT, BIT) \
+  namespace Palatis { \
+    template <> \
+    inline void bitWriteStatic<CHANNEL>(bool const value) { \
+      bitWrite( PORT, BIT, value ); \
+    } \
+  } \
+  SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT)
+
+#define SOFTPWM_DEFINE_CHANNEL_INVERT(CHANNEL, PMODE, PORT, BIT) \
+  namespace Palatis { \
+    template <> \
+    inline void bitWriteStatic<CHANNEL>(bool const value) { \
+      bitWrite(PORT, BIT, !value); \
+    } \
+  } \
+  SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT)
+
+#if defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
+#define SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \
+  namespace Palatis { \
+    CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \
+  } \
+  ISR(TIM1_COMPA_vect) { \
+    interrupts(); \
+    Palatis::SoftPWM.update(); \
+  }
+#else
+	#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega8A__)
+		#define TIMSK1	TIMSK
+	#endif
+#define SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \
+  namespace Palatis { \
+    CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \
+  } \
+  ISR(TIMER1_COMPA_vect) { \
+    interrupts(); \
+	Palatis::SoftPWM.update(); \
+  }
+#endif
+
+#define SOFTPWM_DEFINE_OBJECT(CHANNEL_CNT) \
+  SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, 0)
+
+#define SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \
+  namespace Palatis { \
+    extern CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \
+  }
+
+#define SOFTPWM_DEFINE_EXTERN_OBJECT(CHANNEL_CNT) \
+  SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, 0)
+
+// here comes the magic @o@
+namespace Palatis {
+
+template <int channel> inline void bitWriteStatic(bool value) {}
+template <int channel> inline void pinModeStatic(uint8_t mode) {}
+
+template <int channel>
+struct bitWriteStaticExpander {
+  void operator() (bool value) const {
+    bitWriteStatic<channel>(value);
+    bitWriteStaticExpander < channel - 1 > ()(value);
+  }
+
+  void operator() (uint8_t const &count, uint8_t const * const &channels) const {
+#ifdef SOFTPWM_OUTPUT_DELAY
+    bitWriteStatic<channel>((count + channel) < channels[channel]);
+#else
+    bitWriteStatic<channel>(count < channels[channel]);
+#endif
+    bitWriteStaticExpander < channel - 1 > ()(count, channels);
+  }
+};
+
+template <>
+struct bitWriteStaticExpander < -1 > {
+  void operator() (bool) const {}
+  void operator() (uint8_t const &, uint8_t const * const &) const {}
+};
+
+template <int channel>
+struct pinModeStaticExpander {
+  void operator() (uint8_t const mode) const
+  {
+    pinModeStatic<channel>(mode);
+    pinModeStaticExpander < channel - 1 > ()(mode);
+  }
+};
+
+template <>
+struct pinModeStaticExpander < -1 > {
+  void operator() (uint8_t const mode) const {}
+};
+
+template <unsigned int num_channels, unsigned int num_PWM_levels>
+class CSoftPWM {
+  public:
+    void begin(const unsigned long hertz) {
+      allOff();  //this prevents inverted channels from momentarily going LOW
+      asm volatile ("/************ pinModeStaticExpander begin ************/");
+      const uint8_t oldSREG = SREG;
+      noInterrupts();
+      pinModeStaticExpander < num_channels - 1 > ()( OUTPUT );
+      SREG = oldSREG;
+      asm volatile ("/************ pinModeStaticExpander end ************/");
+
+      /* the setup of timer1 is stolen from ShiftPWM :-P
+         http://www.elcojacobs.com/shiftpwm/ */
+      asm volatile ("/************ timer setup begin ************/");
+      TCCR1A = 0b00000000;
+      TCCR1B = 0b00001001;
+      OCR1A = (F_CPU - hertz * PWMlevels() / 2) / (hertz * PWMlevels());
+      bitSet(TIMSK1, OCIE1A);
+      asm volatile ("/************ timer setup end ************/");
+
+      _count = 0;
+    }
+
+    void set(const int channel_idx, const uint8_t value) {
+      _channels[channel_idx] = value;
+    }
+
+    size_t size() const {
+      return num_channels;
+    }
+
+    unsigned int PWMlevels() const {
+      return num_PWM_levels ? num_PWM_levels : 256;
+    }
+
+    void allOff() {
+      asm volatile ("/********** CSoftPWM::allOff() begin **********/");
+      const uint8_t oldSREG = SREG;
+      noInterrupts();
+      for (int i = 0; i < num_channels; ++i)
+        _channels[i] = 0;
+      bitWriteStaticExpander < num_channels - 1 > ()(false);
+      SREG = oldSREG;
+      asm volatile ("/********** CSoftPWM::allOff() end **********/");
+    }
+
+    /* This function cannot be private because the ISR uses it, and I have
+       no idea about how to make friends with ISR. :-( */
+    void update() __attribute__((always_inline)) {
+      asm volatile ("/********** CSoftPWM::update() begin **********/");
+      const uint8_t count = _count;
+      bitWriteStaticExpander < num_channels - 1 > ()(count, _channels);
+      ++_count;
+      if (_count == PWMlevels())
+        _count = 0;
+      asm volatile ("/********** CSoftPWM::update() end **********/");
+    }
+		
+    #if defined(HAVE_HWSERIAL0)
+      /* this function is stolen from ShiftPWM :-P
+         http://www.elcojacobs.com/shiftpwm/ */
+      void printInterruptLoad() {
+        unsigned long time1, time2;
+
+        bitSet(TIMSK1, OCIE1A); // enable interrupt
+        time1 = micros();
+        delayMicroseconds(5000);
+        time1 = micros() - time1;
+
+        bitClear(TIMSK1, OCIE1A); // disable interrupt
+        time2 = micros();
+        delayMicroseconds(5000);
+        time2 = micros() - time2;
+
+        const double load = static_cast<double>(time1 - time2) / time1;
+        const double interrupt_frequency = static_cast<double>(F_CPU) / (OCR1A + 1);
+        const double cycles_per_interrupt = load * F_CPU / interrupt_frequency;
+
+        Serial.println(F("SoftPWM::printInterruptLoad():"));
+        Serial.print(F("  Load of interrupt: "));
+        Serial.println(load, 10);
+        Serial.print(F("  Clock cycles per interrupt: "));
+        Serial.println( cycles_per_interrupt );
+        Serial.print(F("  Interrupt frequency: "));
+        Serial.print(interrupt_frequency);
+        Serial.println(F(" Hz"));
+        Serial.print(F("  PWM frequency: "));
+        Serial.print(interrupt_frequency / PWMlevels());
+        Serial.println(F(" Hz"));
+        Serial.print(F("  PWM levels: "));
+        Serial.println(PWMlevels());
+
+        bitSet(TIMSK1, OCIE1A);  // enable interrupt again
+      }
+    #endif
+
+  private:
+    uint8_t _channels[num_channels];
+    uint8_t _count;
+};
+
+} // namespace Palatis
+#endif // #ifndef _SOFTPWM_H_
diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
index 0fd30e7..e98c6bc 100644
--- a/debug/motordriver/motordriver.ino
+++ b/debug/motordriver/motordriver.ino
@@ -1,22 +1,32 @@
-//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
-
 
 #include <Adafruit_MCP23X17.h>
+//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/A  front
+#define  m1enA 3   // GPA3
+#define  m1enB 2   // GPA2
+#define  m1pwm1 0  // GPA0
+#define  m1pwm2 1  // GPA1
+#define  m1encA 12 // GPB4
+#define  m1encB 13 // GPB5
+// motor two/B  left
+#define  m2enA 7   // GPA7
+#define  m2enB 6   // GPA6
+#define  m2pwm1 4  // GPA4
+#define  m2pwm2 5  // GPA5
+#define  m2encA 14 // GPB6
+#define  m2encB 15 // GPB7
+// motor three/C    right
+#define  m3enA 11  // GPB3
+#define  m3enB 10  // GPB2
+#define  m3pwm1 8  // GPB0
+#define  m3pwm2 9  // GPB1
+#define  m3encA 24   // teensy port 24
+#define  m3encB 9   // teensy port 9
 
-// 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 sp = 1.000; //speed in 0-1
 
 double dirA = 0.00;
 double dirB = 0.00;
@@ -25,234 +35,230 @@ double spdA = 0.00;
 double spdB = 0.00;
 double spdC = 0.00;
 double dirrad = 0.00;
+double pi = 3.1415926535897;
+
+double Arpm = 0.00;
+volatile long Aencount = 0;
+unsigned long ALtime = 0;
+unsigned long ACtime = 0;
+unsigned long APtime = 0;
+double Brpm = 0.00;
+volatile long Bencount = 0;
+unsigned long BLtime = 0;
+unsigned long BCtime = 0;
+unsigned long BPtime = 0;
+double Crpm = 0.00;
+volatile long Cencount = 0;
+unsigned long CLtime = 0;
+unsigned long CCtime = 0;
+unsigned long CPtime = 0;
+
+Adafruit_MCP23X17 mcp2;
 
 void setup()
 {
+ if (!mcp2.begin_I2C(0x22)) {
+  Serial.println("Error initializing MCP23017 at 0x22.");
+  while (1);
+ }
  // set all the motor control pins to outputs
- pinMode(enA, OUTPUT);
- pinMode(enB, OUTPUT);
- pinMode(enC, OUTPUT);
+ mcp2.pinMode(m1enA, OUTPUT);
+ mcp2.pinMode(m1enB, OUTPUT);
+ mcp2.pinMode(m2enA, OUTPUT);
+ mcp2.pinMode(m2enB, OUTPUT);
+ mcp2.pinMode(m3enA, OUTPUT);
+ mcp2.pinMode(m3enB, OUTPUT);
 
- pinMode(in1, OUTPUT);
- pinMode(in2, OUTPUT);
- pinMode(in3, OUTPUT);
- pinMode(in4, OUTPUT);
- pinMode(in5, OUTPUT);
- pinMode(in6, OUTPUT);
+ mcp2.pinMode(m1pwm1, OUTPUT);
+ mcp2.pinMode(m1pwm2, OUTPUT);
+ mcp2.pinMode(m2pwm1, OUTPUT);
+ mcp2.pinMode(m2pwm2, OUTPUT);
+ mcp2.pinMode(m3pwm1, OUTPUT);
+ mcp2.pinMode(m3pwm2, OUTPUT);
+
+ //set encoder pins to input
+ mcp2.pinMode(m1encA, INPUT);
+ mcp2.pinMode(m1encB, INPUT);
+ mcp2.pinMode(m2encA, INPUT);
+ mcp2.pinMode(m2encB, INPUT);
+ pinMode(m3encA, INPUT);
+ pinMode(m3encB, INPUT);
+ attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE);
 }
-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 softpwm(int pin, int value){
+
+
 }
-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 AencoderISR() {
+  Aencount++;
+}
+void BencoderISR() {
+  Bencount++;
+}
+void CencoderISR() {
+  Cencount++;
+}
+void Areadenc() {
+  ACtime = micros();
+  APtime = ACtime - ALtime;
+  ALtime = ACtime;
+  Arpm = (Aencount / 211.2) * (60000000.0 / APtime);
+  Serial.print("Motor A RPM: ");
+  Serial.print(Arpm);
+  Serial.print("    Sample period:");
+  Serial.print(APtime);
+  Serial.print("ms, encoder count:");
+  Serial.println(Aencount);
+  Aencount = 0;
+}
+void Breadenc() {
+  BCtime = micros();
+  BPtime = BCtime - BLtime;
+  BLtime = BCtime;
+  Brpm = (Bencount / 211.2) * (60000000.0 / BPtime);
+  Serial.print("Motor B RPM: ");
+  Serial.print(Brpm);
+  Serial.print("    Sample period: ");
+  Serial.print(BPtime);
+  Serial.print(" ms, encoder count: ");
+  Serial.println(Bencount);
+  Bencount = 0;
+}
+void Creadenc() {
+  CCtime = micros();
+  CPtime = CCtime - CLtime;
+  CLtime = CCtime;
+  Crpm = (Cencount / 211.2) * (60000000.0 / CPtime);
+  Serial.print("Motor C RPM: ");
+  Serial.print(Crpm);
+  Serial.print("    Sample period: ");
+  Serial.print(CPtime);
+  Serial.print(" ms, encoder count: ");
+  Serial.println(Cencount);
+  Cencount = 0;
 }
 
-void Amove(int sped){
-  Serial.print("moving A ");
+void Amove(double sped) {
+  spdA = abs(sped)*255;
+  mcp2.digitalWrite(m1enA, HIGH);
+  mcp2.digitalWrite(m1enB, LOW);
   if (sped < 0) {
-    digitalWrite(in1, LOW);
-    digitalWrite(in2, HIGH);
-    Serial.print("back");
-  }
-  else{
-    digitalWrite(in1, HIGH);
-    digitalWrite(in2, LOW);
-    Serial.print("forward");
+    Serial.print("Motor A back ");
+    mcp2.analogWrite(m1pwm1, spdA);
+    mcp2.analogWrite(m1pwm2, 0);
+  } else {
+    Serial.print("Motor A forward ");
+    mcp2.analogWrite(m1pwm1, 0);
+    mcp2.analogWrite(m1pwm2, spdA);
   } 
-  Serial.print("with speed: ");
-  sp = abs(sped); 
-  analogWrite(enA, sp);
-  Serial.println(sp);
+  Serial.println(spdA);
 }
-void Bmove(int sped) {
-  Serial.print("moving B ");
+void Bmove(double sped) {
+  spdB = abs(sped)*255;
+  mcp2.digitalWrite(m2enA, HIGH);
+  mcp2.digitalWrite(m2enB, LOW);
   if (sped < 0) {
-    digitalWrite(in3, LOW);
-    digitalWrite(in4, HIGH);
-    Serial.print("back");
+    Serial.print("Motor B back ");
+    mcp2.analogWrite(m2pwm1, spdB);
+    mcp2.analogWrite(m2pwm2, 0);
   } else {
-    digitalWrite(in3, HIGH);
-    digitalWrite(in4, LOW);
-    Serial.print("forward");
+    Serial.print("Motor B forward ");
+    mcp2.analogWrite(m2pwm1, 0);
+    mcp2.analogWrite(m2pwm2, spdB);
   } 
-  Serial.print("with speed: ");
-  int sp = abs(sped);
-  analogWrite(enB, sp);
-  Serial.println(sp);
+  Serial.println(spdB);
 }
-void Cmove(int sped) {
-  Serial.print("moving C ");
+void Cmove(double sped) {
+  spdC = abs(sped) * 255;
+  mcp2.digitalWrite(m3enA, HIGH);
+  mcp2.digitalWrite(m3enB, LOW);
   if (sped < 0) {
-    digitalWrite(in5, LOW);
-    digitalWrite(in6, HIGH);
-    Serial.print("back");
+    Serial.print("Motor C back ");
+    mcp2.analogWrite(m3pwm1, spdC);
+    mcp2.analogWrite(m3pwm2, 0);
   } else {
-    digitalWrite(in5, HIGH);
-    digitalWrite(in6, LOW);
-    Serial.print("forward");
+    Serial.print("Motor C forward ");
+    mcp2.analogWrite(m3pwm1, 0);
+    mcp2.analogWrite(m3pwm2, spdC);
   } 
-  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);
+  Serial.println(spdC);
 }
 
 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();
+  Amove(0);
+  Bmove(0);
+  Cmove(0);
 
-  double dirrad = (direction*71) / 4068;  
+  double dirrad = (direction*71.0000) / 4068.0000;  
   Serial.println(dirrad);
   //vector ontb in x en y
-  double x = sin(dirrad)*speed;
-  double y = cos(dirrad)*speed;
+  double y = sin(dirrad);
+  double x = cos(dirrad);
   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);
+  double spdeA = y*speed;
+  double spdeB = (-0.500*x - 0.8660054*y)*speed;
+  double spdeC = (-0.500*x + 0.8660054*y)*speed;
+  Amove(spdeA);
+  Bmove(spdeB);
+  Cmove(spdeC);
   Serial.println();
   Serial.println();
 }
-void loop()
-{
-  //demoOne();
-  //delay(1000);
-  //demoTwo();
+void rotate(int rodeg, int speed){
+//use compas heading
+}
+void demo(){
+  Amove(sp);
+  Bmove(sp);
+  Cmove(sp); 
+  Serial.println("seperate A B C rotation");
   delay(1000);
-  turnoff();
-  beddermoveohmi(255, 0);
+  
+  Amove(0);
+  Bmove(0);
+  Cmove(0);
+  Serial.println("seperate A B C stop");
   delay(1000);
-  turnoff();
-  Amove(255);
+
+  Amove(-sp);
+  Bmove(-sp);
+  Cmove(-sp);
+  Serial.println("seperate A B C -rotation");
+  delay(1000);
+
+  moveohmi(0, 0);
+  Serial.println("ohmi stop");
   delay(1000);
-  turnoff();
-  Bmove(255);
+
+  rotate(90, sp);
+  Serial.println("rotation function 90deg");
+  rotate(90, sp);
+  Serial.println("rotation function -90deg");
   delay(1000);
-  turnoff();
-  Cmove(255);
+
+  moveohmi(sp, 35);
+  Serial.println("ohmi move 35deg");
   delay(1000);
-  turnoff();
-  turn(254);
+
+  moveohmi(sp, 240);
+  Serial.println("ohmi move 215deg");
   delay(1000);
+
+  for(int i = 0; i<360; i++){
+    moveohmi(sp, i);
+    Serial.println(i);
+    delay(50);
+  }
+  Serial.println("ohmi move circle");
+  delay(1000);
+}
+void loop(){
+  demo();
+  delay(100);
+
 }
diff --git a/debug/motordriver/nomultiplexer/nomultiplexer.ino b/debug/motordriver/nomultiplexer/nomultiplexer.ino
index 4ca1ca8..abfe14c 100644
--- a/debug/motordriver/nomultiplexer/nomultiplexer.ino
+++ b/debug/motordriver/nomultiplexer/nomultiplexer.ino
@@ -190,9 +190,8 @@ void moveohmi(int speed, int direction){
   double x = cos(dirrad);
   Serial.println((String)"x:"+x+" y:"+y);
   double spdeA = y*speed;
-  double spdeB = (-0.500*x - sqrt(3.000)/2.000*y)*speed;
-  double spdeC = (-0.500*x + sqrt(3.000)/2.000*y)*speed;
-  Serial.println(spdeA);
+  double spdeB = (-0.500*x - 0.8660054*y)*speed;
+  double spdeC = (-0.500*x + 0.8660054*y)*speed;
   Amove(spdeA);
   Bmove(spdeB);
   Cmove(spdeC);
@@ -203,7 +202,6 @@ void rotate(int rodeg, int speed){
 //use compas heading
 }
 void demo(){
-  
   Amove(sp);
   Bmove(sp);
   Cmove(sp); 
@@ -235,8 +233,8 @@ void demo(){
   moveohmi(sp, 35);
   Serial.println("ohmi move 35deg");
   delay(1000);
-  
-  moveohmi(sp, 215);
+
+  moveohmi(sp, 240);
   Serial.println("ohmi move 215deg");
   delay(1000);
 
@@ -247,7 +245,6 @@ void demo(){
   }
   Serial.println("ohmi move circle");
   delay(1000);
- 
 }
 void loop(){
   demo();