From 21e29aa564c6334d3b01fc728a7aa4032de4b938 Mon Sep 17 00:00:00 2001
From: HappyZ <zhuyanzi@gmail.com>
Date: Sat, 6 Feb 2016 22:03:28 -0800
Subject: [PATCH] ideally 0.83cm per move

cannot really accurately make it to 1cm.. either 0.83cm or 1.2cm-ish.
---
 Test/Test.ino | 52 +++++++++++++++++++--------------------------------
 1 file changed, 19 insertions(+), 33 deletions(-)

diff --git a/Test/Test.ino b/Test/Test.ino
index e2178f1..dc9e43c 100644
--- a/Test/Test.ino
+++ b/Test/Test.ino
@@ -16,63 +16,49 @@ MotorWheel wheel2(11,12,6,7,&irq2,REDUCTION_RATIO,int(144*PI));
 
 R2WD _2WD(&wheel1,&wheel2,WHEELSPAN);
 
-bool myLED = HIGH;
+//bool myLED = HIGH;
 String inputString = "";
+String prevString = "";
 boolean stringComplete = false;
 
-unsigned int speedMMPS=150;
-unsigned int uptime=110;
+unsigned int speedMMPS=83;
+unsigned int uptime=300;
 unsigned int currentSpeed = 0;
 
-unsigned int myCounter = 0;
-
-
 void setup() {
     Serial.begin(9600);
     // reserve 200 bytes for the inputString:
-    inputString.reserve(50);
+    inputString.reserve(20);
+    prevString.reserve(20);
     //TCCR0B=TCCR0B&0xf8|0x01;    // warning!! it will change millis()
     TCCR1B=TCCR1B&0xf8|0x01;    // Pin9,Pin10 PWM 31250Hz
     TCCR2B=TCCR2B&0xf8|0x01;    // Pin3,Pin11 PWM 31250Hz
     _2WD.PIDEnable(0.35,0.02,0,10);
 
-    pinMode(13, OUTPUT);
 }
 
 void loop() {
     if (stringComplete) {
-        Serial.println("You input: "+inputString);
-        Serial.println(myCounter);
-        Serial.write(myCounter);
-
+//        Serial.println("You input: "+inputString);
         if (inputString.equals("go\n")) {
-            _2WD.setCarAdvance(0);
+            if (!inputString.equals(prevString))
+                _2WD.setCarAdvance(0);
             currentSpeed = _2WD.setCarSpeedMMPS(speedMMPS,uptime);
-            Serial.println("Current speed: "+String(currentSpeed));
+//            Serial.println("Current speed: "+String(currentSpeed));
             _2WD.setCarSlow2Stop(uptime);
-            Serial.println("Current speed stop: "+String(_2WD.getCarSpeedMMPS()));
-            myCounter += 1;
-            digitalWrite(13, myLED);   // turn the LED on (HIGH is the voltage level)
-            if (myLED == HIGH)
-                myLED = LOW;
-            else
-                myLED = HIGH;
+//            Serial.println("Current speed stop: "+String(_2WD.getCarSpeedMMPS()));
         } else if (inputString.equals("back\n")) {
-            _2WD.setCarBackoff(0);
+            if (!inputString.equals(prevString))
+                _2WD.setCarBackoff(0);
             currentSpeed = _2WD.setCarSpeedMMPS(speedMMPS,uptime);
-            Serial.println("Current speed: "+String(currentSpeed));
+//            Serial.println("Current speed: "+String(currentSpeed));
             _2WD.setCarSlow2Stop(uptime);
-            Serial.println("Current speed stop: "+String(_2WD.getCarSpeedMMPS()));
-            myCounter += 1;
-            digitalWrite(13, myLED);   // turn the LED on (HIGH is the voltage level)
-            if (myLED == HIGH)
-                myLED = LOW;
-            else
-                myLED = HIGH;
+//            Serial.println("Current speed stop: "+String(_2WD.getCarSpeedMMPS()));
+        } else if (inputString.equals("speed\n")) {
+//            Serial.println("Current speed: "+String(_2WD.getCarSpeedMMPS()));
         } 
-        
-        
-        
+//        _2WD.demoActions(speedMMPS, 1000, 500);
+        prevString = inputString;
         // clear the string:
         inputString = "";
         stringComplete = false;