refactoring throttle

… adds buttery smoothness

Wiiceiver/ElectronicSpeedController.h

@@ -11,43 +11,56 @@ class ElectronicSpeedController {
 
 private:
   Servo _esc;
+  int angle;   // the angle most recently written to _esc;
 
 public:
 
 void init(int pin) {
+  angle = ESC_CENTER;
+
 #ifdef DEBUGGING_ESC
   Serial.print("attaching to pin #");
   Serial.println(pin);
 #endif
 
   _esc.attach(pin);
+
 #ifdef DEBUGGING
   Serial.println("initializing ESC...");
 #endif
   for (int i = 0; i < 1000; i+= 20) {
     _esc.write(ESC_CENTER);
     delay(20);
   }
-  // _sweep();
+
+  delay(100);
 #ifdef DEBUGGING_ESC
   Serial.println("done");
 #endif
 }
 
 
-// input: -1 .. 1
-// output: 0 .. 180
+/*
+ * input: -1 .. 1
+ * output: writes +/- ESC_MAX_ANGLE to _esc
+ * does *not* write the same angle twice -- possible interference with the PWM :(
+ */
 void setLevel(float level) {
-  int angle = (int)(ESC_CENTER + (ESC_MAX_ANGLE - ESC_CENTER) * level);
+  int newAngle = (int)(ESC_CENTER + (ESC_MAX_ANGLE - ESC_CENTER) * level);
+  if (newAngle != angle) {
 #ifdef DEBUGGING_ESC
-  Serial.print(F("ESC angle: "));
-  Serial.println(angle);
+    Serial.print(millis());
+    Serial.print(F(": ESC angle: "));
+    Serial.println(newAngle);
 #endif
-  _esc.write(angle);
-}
+    angle = newAngle;
+    _esc.write(angle);
+  }
+} // void setLevel(float level)
 
 
 private:
+// unused code
 #define STEP_DELAY 20
   void _sweep(void) {
     setLevel(0);
@@ -66,7 +79,7 @@ void setLevel(float level) {
     }
     setLevel(0);
     delay(STEP_DELAY);
-  }
-};
+  } // void _sweep(void)
+};  // class ElectronicSpeedController 
 
 #endif

Wiiceiver/Smoother.h

@@ -1,20 +1,24 @@
 #ifndef SMOOTHER_H
 #define SMOOTHER_H
 
+/*
+ * A helper class -- smooths the throttle input
+ * 
+ */
+
 class Smoother {
   private:
     float value;
-    float factor;
 
   public:
-    Smoother(float newFactor) {
-      factor = newFactor;
+    Smoother(void) {
       value = 0;
     }
+
 
-#define MIN_STEP 0.005
+#define MIN_STEP 0.003
 
-    float compute(float target) {
+    float compute(float target, float factor) {
       float step = (target - value) * factor;
 
 #ifdef DEBUGGING_SMOOTHER
@@ -42,11 +46,13 @@ class Smoother {
       Serial.println(value, 4);
 #endif
       return value;
-    }
-
+    }  // float compute(float target, float factor)
+
+
+    // reset the internal smoothing value, to quickly seek zero
     void zero() {
       value = 0;
-    }
+    } // void zero()
 };
 
 #endif

Wiiceiver/Throttle.h

@@ -0,0 +1,96 @@
+#ifndef THROTTLE_H
+#define THROTTLE_H
+
+// not strictly necessary, but a nice reminder
+#include "Chuck.h"
+#include "Smoother.h"
+
+/*
+ * Manages the throttle input; presents a smoothed output, [ -1 .. 1 ]
+ */
+
+class Throttle {
+  private:
+    float throttle, smoothed;
+    Smoother smoother;
+
+  public:    
+
+    Throttle() {
+      smoother = Smoother();
+      throttle = 0;
+    } // Throttle()
+
+
+    /*
+     * returns a smoothed float [-1 .. 1]
+     *
+     * Theory of Operation: identify the throttle position (joystick angle), 
+     *   then return a smoothed representation
+     *
+     *   if C is pressed, "cruise control":
+     *      set "cruise" to last joystick position
+     *      if joystick == up, increment throttle position
+     *      if joystick == down, decrement throttle position
+     *   else throttle position == chuck.Y joystick position
+     *   return a smoothed value from the throttle position
+     */
+    float update(Chuck chuck) {
+#ifdef DEBUGGING_THROTTLE
+      Serial.print("Throttle: ");
+      Serial.print("y=");
+      Serial.print(chuck.Y, 4);
+      Serial.print(", ");
+      Serial.print("c=");
+      Serial.print(chuck.C);
+      Serial.print("; ");
+#endif
+
+      if (chuck.C) { // cruise control!
+#ifdef DEBUGGING_THROTTLE
+        Serial.print("CC: last = ");
+        Serial.print(throttle, 4);
+        Serial.print(", ");
+#endif
+        // throttle = lastThrottle;
+        if (chuck.Y > 0.5 && throttle < 1.0) {
+          throttle += THROTTLE_CC_BUMP;
+        } else if (chuck.Y < -0.5 && throttle > -1.0) {
+          throttle -= THROTTLE_CC_BUMP;
+        } 
+
+      } else {
+        throttle = chuck.Y;
+
+        // "center" the joystick by enforcing some very small minimum
+        if (abs(throttle) < MIN_THROTTLE) {
+          throttle = 0;
+        }
+      } // if (chuck.C) -- else
+
+      smoothed = smoother.compute(throttle, chuck.Z ? THROTTLE_SMOOTHNESS : THROTTLE_SMOOTHNESS / 4);
+
+#ifdef DEBUGGING_THROTTLE
+      Serial.print(F("position: "));
+      Serial.print(throttle, 4);
+      Serial.print(F(" smoothed: "));
+      Serial.println(smoothed, 4);
+#endif
+
+      return smoothed;
+    } // float update(void)
+
+
+    float getThrottle(void) {
+      return smoothed;
+    } // float getThrottle()
+
+
+    void zero(void) {
+      throttle = 0;
+      smoother.zero();
+    } // void zero(void)
+
+}; // class Throttle
+
+#endif