diff --git a/Faraday.NODEMCU/Faraday.Discbrakes.ino b/Faraday.NODEMCU/Faraday.Discbrakes.ino deleted file mode 100644 index 3dff553..0000000 --- a/Faraday.NODEMCU/Faraday.Discbrakes.ino +++ /dev/null @@ -1,34 +0,0 @@ -#if defined(ENABLEDISCBRAKE) -void setupDiscBrake() -{ - //Brake1 - pinMode(PINSERVOBRAKE1, OUTPUT); - //Brake2 - pinMode(PINSERVOBRAKE2, OUTPUT); - - Servo servoDiscBrake1; - Servo servoDiscBrake2; - servoDiscBrake1.attach(PINSERVOBRAKE1, servoMinPWM, servoMaxPWM); - servoDiscBrake2.attach(PINSERVOBRAKE2, servoMinPWM, servoMaxPWM); -} - - -void readDiscCurrentSensor() -{ - Serial.print("Reading Sensor: "); - Serial.println(analogRead(A0)); -} - -int readAnalogSensorPin(byte pin) -{ - byte measurements = 5; - float totalMeasurement = 0; - for (byte i = 0; i < measurements; i++) - { - totalMeasurement += analogRead(A0); - delay(1); - } - return totalMeasurement / measurements; -} -#endif - diff --git a/Faraday.NODEMCU/Faraday.LED.ino b/Faraday.NODEMCU/Faraday.LED.ino index 1c6d0de..136a3e0 100644 --- a/Faraday.NODEMCU/Faraday.LED.ino +++ b/Faraday.NODEMCU/Faraday.LED.ino @@ -1,8 +1,18 @@ #if defined(ENABLELED) +byte lastMotorDirection = -1; + void setupLeds() { - leds.init(LEDSTOTAL); + lastMotorDirection = motorDirection; + leds.Begin(); + showLedLights(); +} + +void updateLeds() +{ + if (leds.CanShow()) + leds.Show(); } void showLedState() @@ -22,13 +32,20 @@ void showLedLights() //Front lights for (int i = LEDSFRONTINDEX; i < LEDSFRONTINDEX + LEDSFRONTCOUNT; i++) { setPixelColor(i, 100, 100, 100); + yield(); } - for (int i = LEDSBACKINDEX; i < LEDSBACKINDEX + LEDSBACKCOUNT; i++) { - //If braking, put brake lights - if (motorDirection == 2) - setPixelColor(i, 255, 0, 0); - else - setPixelColor(i, 100, 0, 0); + if (lastMotorDirection != motorDirection) + { + for (int i = LEDSBACKINDEX; i < LEDSBACKINDEX + LEDSBACKCOUNT; i++) { + //If braking, put brake lights + if (motorDirection == 2) + setPixelColor(i, 255, 0, 0); + else + setPixelColor(i, 100, 0, 0); + yield(); + } + updateLeds(); + lastMotorDirection = motorDirection; } } @@ -80,64 +97,67 @@ void setLedControls() } } } - byte readyColorCounter = 0; - byte readyColor = 1; - byte readyColorStep = 3; - const byte readyColorMin = 0; - const byte readyColorMax = 90; +byte readyColorCounter = 0; +byte readyColor = 1; +byte readyColorStep = 3; +const byte readyColorMin = 0; +const byte readyColorMax = 90; - void ledReadyState() - { - for (int i = LEDSCONTROLINDEX; i < LEDSCONTROLCOUNT; i++) { - pixels[i] = Wheel(readyColorCounter); - } - if (readyColorCounter <= readyColorMin) - readyColor = +readyColorStep; - else if (readyColorCounter >= readyColorMax) - readyColor = -readyColorStep; - readyColorCounter += readyColor; +void ledReadyState() +{ + for (int i = LEDSCONTROLINDEX; i < LEDSCONTROLCOUNT; i++) { + leds.SetPixelColor(i, Wheel(readyColorCounter)); + yield(); } + if (readyColorCounter <= readyColorMin) + readyColor = +readyColorStep; + else if (readyColorCounter >= readyColorMax) + readyColor = -readyColorStep; + readyColorCounter += readyColor; +} - void ledDeadState() +void ledDeadState() +{ + uint8_t i; + static uint8_t c = 0; + static uint8_t d = 5; + for (i = LEDSCONTROLINDEX; i < LEDSCONTROLCOUNT; i++) { - uint8_t i; - static uint8_t c = 0; - static uint8_t d = 5; - for (i = LEDSCONTROLINDEX; i < LEDSCONTROLCOUNT; i++) - setPixelColor(i, c, 0, 0); - c += d; - if ( (c >= 255) || (c <= 0) ) d = -d; + setPixelColor(i, c, 0, 0); + yield(); } + c += d; + if ( (c >= 255) || (c <= 0) ) d = -d; +} - void setPixelColor(byte index, byte r, byte g, byte b) - { - pixels[index].R = r; - pixels[index].G = g; - pixels[index].B = b; - } +void setPixelColor(byte index, byte r, byte g, byte b) +{ + RgbColor pixel(r, g, b); + leds.SetPixelColor(index, pixel); +} - Pixel_t Wheel(byte WheelPos) { - WheelPos = 255 - WheelPos; - Pixel_t p; - if (WheelPos < 85) { - p.R = 255 - WheelPos * 3; - p.G = 0; - p.B = WheelPos * 3; - return p; - } else if (WheelPos < 170) { - WheelPos -= 85; - p.R = 0; - p.G = WheelPos * 3; - p.B = 255 - WheelPos * 3; - return p; - } else { - WheelPos -= 170; - p.R = WheelPos * 3; - p.G = 255 - WheelPos * 3; - p.B = 0; - return p; - } - } +RgbColor Wheel(byte WheelPos) { + WheelPos = 255 - WheelPos; + RgbColor p; + if (WheelPos < 85) { + p.R = 255 - WheelPos * 3; + p.G = 0; + p.B = WheelPos * 3; + return p; + } else if (WheelPos < 170) { + WheelPos -= 85; + p.R = 0; + p.G = WheelPos * 3; + p.B = 255 - WheelPos * 3; + return p; + } else { + WheelPos -= 170; + p.R = WheelPos * 3; + p.G = 255 - WheelPos * 3; + p.B = 0; + return p; + } +} #endif diff --git a/Faraday.NODEMCU/Faraday.NODEMCU.ino b/Faraday.NODEMCU/Faraday.NODEMCU.ino index 3752564..d34bd40 100644 --- a/Faraday.NODEMCU/Faraday.NODEMCU.ino +++ b/Faraday.NODEMCU/Faraday.NODEMCU.ino @@ -1,9 +1,9 @@ -//Requires -//http://arduino.esp8266.com/stable/package_esp8266com_index.json -//VERSION: 2.1.0 +//Requires +//http://arduino.esp8266.com/staging/package_esp8266com_index.json +//VERSION: 2.2.0-RC1 //ArduinoNunchuk //Metro -//WS2812 +//NeoPixelBus //Faraday.Vesc //Optional defines @@ -14,14 +14,16 @@ #define ENABLEWIFI //Enable wifi AP #define ENABLENUNCHUK //Enable control through a nunchuk //#define ENABLELED //Enable led control - NOT WORKING RELIABLE -//#define ENABLEDEADSWITCH //Enable dead man switch +//#define ENABLEDEADSWITCH //Enable dead man switch //#define ENABLEOTAUPDATE //OTA - Not working //#define ENABLEDISCBRAKE //Disc brakes - not working #define ENABLESMOOTHING //Enable smothing of input values #define ENABLENONLINEARBRAKE // Non linear braking, softer braking in the beginning +#define FARADAY_VERSION "NODEMCU20160505-2.2.0-RC1" + //How many clients should be able to connect to this ESP8266 -#define MAX_SRV_CLIENTS 1 +#define MAX_SRV_CLIENTS 5 //Pins #define PINEXTERNALRESET 16 @@ -29,6 +31,7 @@ #define PINSERVOESC 0 #define PINSERVOBRAKE1 2 #define PINSERVOBRAKE2 14 +#define PINPIXEL 3 //Required includes #include @@ -38,7 +41,8 @@ #include #include #include -#include +#include +//#include #include #include "FS.h" @@ -81,16 +85,15 @@ Servo servoESC; #endif #if defined(ENABLELED) -#define LEDSTOTAL 14 -#define LEDSCONTROLCOUNT 12 -#define LEDSFRONTCOUNT 1 -#define LEDSBACKCOUNT 1 +#define LEDSTOTAL 24 +#define LEDSCONTROLCOUNT 0 +#define LEDSFRONTCOUNT 0 +#define LEDSBACKCOUNT 24 #define LEDSCONTROLINDEX 0 -#define LEDSFRONTINDEX 12 -#define LEDSBACKINDEX 13 +#define LEDSFRONTINDEX 0 +#define LEDSBACKINDEX 0 -static WS2812 leds; -static Pixel_t pixels[LEDSTOTAL]; +NeoPixelBus leds(LEDSTOTAL, PINPIXEL); #endif #if defined(ENABLEWEBUPDATE) @@ -179,9 +182,9 @@ void setupSERVO() void setup() { Serial.begin(115200); - delay(1000); + delay(250); loadConfiguration(); - + #if defined(ENABLEWIFI) setupWIFI(); #endif @@ -193,10 +196,9 @@ void setup() #endif pinMode(PINEXTERNALRESET, OUTPUT); digitalWrite(PINEXTERNALRESET, LOW); - delay(250); - Serial.print("Faraday Motion FirmwareVersion:"); - Serial.println(faradayVersion); + Serial.print(F("Faraday Motion FirmwareVersion:")); + Serial.println(FARADAY_VERSION); #if defined(ENABLEOTAUPDATE) setupOTA(); @@ -255,14 +257,14 @@ void loop() #endif yield(); - if (metroControllerRead.check() == 1) { + if (metroControllerRead.check() == 1) { #if defined(ENABLEWIFI) - readFromWifiClient(); + readFromWifiClient(); #endif #if defined(ENABLENUNCHUK) - readFromNunchukClient(); + readFromNunchukClient(); #endif - delay(1); + yield(); #if defined(ENABLEDEADSWITCH) if (digitalRead(PINDEADSWITCH) == HIGH) @@ -302,14 +304,16 @@ void loop() #if defined(ENABLELED) if (metroLeds.check() == 1) { - setLedControls(); - yield(); - } - if (metroLedsReadyState.check() == 1) - { - showLedState(); - yield(); - leds.show(pixels); + //setLedControls(); + showLedLights(); } + // if (metroLedsReadyState.check() == 1) + // { + // showLedState(); + // yield(); + // } + // updateLeds(); #endif -} +} + + diff --git a/Faraday.NODEMCU/Faraday.NunchukClient.ino b/Faraday.NODEMCU/Faraday.NunchukClient.ino index 138d5e0..204360a 100644 --- a/Faraday.NODEMCU/Faraday.NunchukClient.ino +++ b/Faraday.NODEMCU/Faraday.NunchukClient.ino @@ -17,7 +17,7 @@ byte defaultNunchukNeutral = 127; void readFromNunchukClient() { nunchuk.update(); - delay(1); + yield(); #if defined(ENABLEDEVMODE) Serial.println("DATA"); Serial.print(nunchuk.analogX, DEC); diff --git a/Faraday.NODEMCU/Faraday.OTA.ino b/Faraday.NODEMCU/Faraday.OTA.ino deleted file mode 100644 index d2fd711..0000000 --- a/Faraday.NODEMCU/Faraday.OTA.ino +++ /dev/null @@ -1,28 +0,0 @@ -#if defined(ENABLEOTAUPDATE) -void setupOTA() -{ - ArduinoOTA.onStart([]() { - Serial.println("Start"); - }); - - ArduinoOTA.onEnd([]() { - Serial.println("\nEnd"); - }); - - ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { - Serial.printf("Progress: %u%%\r", (progress / (total / 100))); - }); - - ArduinoOTA.onError([](ota_error_t error) { - Serial.printf("Error[%u]: ", error); - if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); - else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); - else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); - else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); - else if (error == OTA_END_ERROR) Serial.println("End Failed"); - }); - - ArduinoOTA.begin(); -} -#endif - diff --git a/Faraday.NODEMCU/Faraday.WifiClient.ino b/Faraday.NODEMCU/Faraday.WifiClient.ino index 424798f..b0d2d1b 100644 --- a/Faraday.NODEMCU/Faraday.WifiClient.ino +++ b/Faraday.NODEMCU/Faraday.WifiClient.ino @@ -6,12 +6,13 @@ void setupWIFI() IPAddress address(10, 10, 100, 254); IPAddress subnet(255, 255, 255, 0); + //If we dont disable the wifi, then there will be some issues with conncting to the device sometimes + WiFi.disconnect(true); byte channel = 11; float wifiOutputPower = 20.5; //Max power WiFi.setOutputPower(wifiOutputPower); WiFi.setPhyMode(WIFI_PHY_MODE_11B); WiFi.setSleepMode(WIFI_NONE_SLEEP); - WiFi.disconnect(true); WiFi.mode(WIFI_AP); //C:\Users\spe\AppData\Roaming\Arduino15\packages\esp8266\hardware\esp8266\2.1.0\cores\esp8266\core_esp8266_phy.c //TRYING TO SET [114] = 3 in core_esp8266_phy.c 3 = init all rf @@ -26,7 +27,7 @@ void setupWIFI() server.begin(); //Set delay = true retarts the esp in version 2.1.0, check in later versions if its fixed - //server.setNoDelay(true); + server.setNoDelay(true); } void hasClients() @@ -99,7 +100,7 @@ void readFromWifiClient() { m[packetCount++] = serverClients[i].read(); } - delay(1); + yield(); #if defined(ENABLEDEVMODE) @@ -120,7 +121,7 @@ void readFromWifiClient() #endif if (validateChecksum(m, packetCount)) { - delay(1); + yield(); //Set the power and specify controller 1, the wifi reciever if (controlType == 0 || controlType == 1) {