From 7ff356690bb3f2f675dfb6b44fd33b23aa14006a Mon Sep 17 00:00:00 2001 From: Austin David Date: Sat, 10 May 2014 16:12:51 -0700 Subject: [PATCH] refactoring throttle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit … adds buttery smoothness --- Wiiceiver/ElectronicSpeedController.h | 33 ++++--- Wiiceiver/Smoother.h | 22 +++-- Wiiceiver/Throttle.h | 96 ++++++++++++++++++++ Wiiceiver/Wiiceiver.ino | 124 ++++++++++---------------- 4 files changed, 178 insertions(+), 97 deletions(-) create mode 100644 Wiiceiver/Throttle.h diff --git a/Wiiceiver/ElectronicSpeedController.h b/Wiiceiver/ElectronicSpeedController.h index c8c0495..6d43b17 100644 --- a/Wiiceiver/ElectronicSpeedController.h +++ b/Wiiceiver/ElectronicSpeedController.h @@ -11,16 +11,20 @@ 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 @@ -28,26 +32,35 @@ void init(int pin) { _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 diff --git a/Wiiceiver/Smoother.h b/Wiiceiver/Smoother.h index bb6ed47..07cbfbb 100644 --- a/Wiiceiver/Smoother.h +++ b/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 diff --git a/Wiiceiver/Throttle.h b/Wiiceiver/Throttle.h new file mode 100644 index 0000000..fd8ad6f --- /dev/null +++ b/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 diff --git a/Wiiceiver/Wiiceiver.ino b/Wiiceiver/Wiiceiver.ino index c53c921..3e1b25e 100644 --- a/Wiiceiver/Wiiceiver.ino +++ b/Wiiceiver/Wiiceiver.ino @@ -1,16 +1,34 @@ +/* + * Wiiceiver hardware definitions + * don't change these without changing the board + */ +#define GREEN_LED 7 // any digital pin; DIP13 +#define RED_LED 8 // any digital pin; DIP14 + +#define ESC_PPM 10 // PWM required; DIP16 +#define ESC_GROUND 00 // hard-wired + +#define WII_SCL 19 // aka A5 -- DO NOT USE (Wire needs it) +#define WII_SDA 18 // aka A4 -- DO NOT USE (Wire needs it) +#define WII_POWER 9 // any digital pin; DIP15 +#define WII_GROUND 00 // hard-wired + + + + #include #include #include -#define DEBUGGING +// #define DEBUGGING #include "Blinker.h" // #define DEBUGGING_SMOOTHER #include "Smoother.h" -#define DEBUGGING_CHUCK +// #define DEBUGGING_CHUCK // #define DEBUGGING_CHUCK_ACTIVITY #define WII_ACTIVITY_COUNTER 100 // once per 20ms; 50 per second #include "Chuck.h" @@ -18,32 +36,21 @@ // #define DEBUGGING_ESC #include "ElectronicSpeedController.h" - // #define DEBUGGING_THROTTLE +#define MIN_THROTTLE 0.05 // the lowest throttle to send the ESC +#define THROTTLE_CC_BUMP 0.002 // CC = 0.1% throttle increase; 50/s = 10s to hit 100% on cruise +#define THROTTLE_SMOOTHNESS 0.2 // default "smoothing" factor +#include "Throttle.h" -/* - * Wiiceiver hardware definitions - * don't change these without changing the board - */ -#define GREEN_LED 7 // any digital pin; DIP13 -#define RED_LED 8 // any digital pin; DIP14 - -#define ESC_PPM 10 // PWM required; DIP16 -#define ESC_GROUND 00 // hard-wired - -#define WII_SCL 19 // aka A5 -- DO NOT USE (Wire needs it) -#define WII_SDA 18 // aka A4 -- DO NOT USE (Wire needs it) -#define WII_POWER 9 // any digital pin; DIP15 -#define WII_GROUND 00 // hard-wired -Chuck chuck = Chuck(); +Chuck chuck; ElectronicSpeedController ESC; Blinker green = Blinker(GREEN_LED); Blinker red = Blinker(RED_LED); -Smoother smoother = Smoother(0.2); +Throttle throttle; // maybe calibrate the joystick: @@ -104,14 +111,14 @@ void splashScreen() { // flash the LEDs to indicate throttle position -void updateLEDs(float throttle) { - if (throttle == 0) { +void updateLEDs(Throttle throttle) { + if (throttle.getThrottle() == 0) { green.update(1); red.update(1); } else { - int bps = abs(int(throttle * 20)); + int bps = abs(int(throttle.getThrottle() * 20)); - if (throttle > 0) { + if (throttle.getThrottle() > 0) { green.update(bps); red.update(1); } else { @@ -210,55 +217,14 @@ bool startChuck() { -#define MIN_THROTTLE 0.05 // the lowest throttle to send the ESC -#define THROTTLE_CC_BUMP 0.003 // CC = 0.1% throttle increase; 50/s = 10s to hit 100% on cruise -float lastThrottle = 0; // global-ish -float getThrottle() { - float throttle; - -#ifdef DEBUGGING_THROTTLE - Serial.print("Throttle "); -#endif - - if (chuck.C) { // cruise control! -#ifdef DEBUGGING_THROTTLE - Serial.print("CC: last = "); - Serial.print(lastThrottle); - 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; - - if (abs(throttle) < MIN_THROTTLE) { - throttle = 0; - } - } - -#ifdef DEBUGGING_THROTTLE - Serial.print("setting "); - Serial.println(throttle); -#endif - - lastThrottle = throttle; - return smoother.compute(throttle); -} // float getThrottle() - - - void handleInactivity() { #ifdef DEBUGGING Serial.print(millis()); Serial.println(": handling inactivity"); #endif - lastThrottle = 0; // kills cruise control - smoother.zero(); // kills throttle history + // lastThrottle = 0; // kills cruise control + // smoother.zero(); // kills throttle history + throttle.zero(); ESC.setLevel(0); do { freakOut(); @@ -301,8 +267,10 @@ void setup() { red.init(); setup_pins(); - splashScreen(); + ESC.init(ESC_PPM); + splashScreen(); + delay(5000); // hold for nunchuck powerup #ifdef DEBUGGING @@ -310,10 +278,10 @@ void setup() { #endif green.high(); red.high(); - if (! startChuck()) { - handleInactivity(); - } else { + if (startChuck()) { maybeCalibrate(); + } else { + handleInactivity(); } #ifdef DEBUGGING Serial.println("Nunchuck is active!"); @@ -324,14 +292,12 @@ void setup() { green.update(1); red.update(1); - - ESC.init(ESC_PPM); } // void setup() void loop() { - static float lastThrottle = 0; + static float lastThrottleValue = 0; unsigned long startMS = millis(); green.run(); red.run(); @@ -344,10 +310,10 @@ void loop() { handleInactivity(); // delay(100); } else { - float throttle = getThrottle(); - if (throttle != lastThrottle) { + float throttleValue = throttle.update(chuck); + ESC.setLevel(throttleValue); + if (throttleValue != lastThrottleValue) { updateLEDs(throttle); - ESC.setLevel(throttle); #ifdef DEBUGGING Serial.print("y="); Serial.print(chuck.Y, 4); @@ -357,9 +323,9 @@ void loop() { Serial.print(", z="); Serial.print(chuck.Z); Serial.print(", "); - Serial.println(throttle, 4); + Serial.println(throttleValue, 4); #endif - lastThrottle = throttle; + lastThrottleValue = throttleValue; } int delayMS = constrain(startMS + 20 - millis(), 5, 20); // Serial.print("sleeping "); Serial.println(delayMS);