diff --git a/.gitignore b/.gitignore index 89cc49c..afe1a39 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +lib \ No newline at end of file diff --git a/data/config.json b/data/config.json new file mode 100644 index 0000000..4ae1bb3 --- /dev/null +++ b/data/config.json @@ -0,0 +1,16 @@ +{ + "cartname": "TarantlCart_000", + "ssid": "TarantlBros", + "passwd": "chillfumml", + "speedprofile":[ + { + "speed_max":-300, + "speed_min":300, + "steer_max":200, + "steer_min":-200, + "accel":200, + "boost":100, + "symbol":0 + } + ] +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index cf5b211..9984bc4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,6 +12,6 @@ platform = espressif32 board = m5stack-core-esp32 framework = arduino -lib_deps = madhephaestus/WiiChuck @ ^0.3.2, siteswapjuggler/Ramp @ ^0.6.0 +lib_deps = madhephaestus/WiiChuck @ ^0.3.2, siteswapjuggler/Ramp @ ^0.6.0, ayushsharma82/AsyncElegantOTA @ ^2.2.6, me-no-dev/AsyncTCP @ ^1.1.1, me-no-dev/ESP Async WebServer @ ^1.2.3, WiFi, bblanchon/ArduinoJson @ ^6.19.4 ;lib_deps = M5Stack, siteswapjuggler/Ramp @ ^0.6.0 monitor_speed = 115200 diff --git a/src/buttons.h b/src/buttons.h new file mode 100644 index 0000000..e57e4a3 --- /dev/null +++ b/src/buttons.h @@ -0,0 +1,29 @@ + if (M5.BtnA.wasReleased()) { + //motorOn = true; + myDrive = 0; + setDisplay(myDrive, true); + } + + if (M5.BtnA.pressedFor(1000)) { + setDisplay(myDrive, true); + } + + if (M5.BtnB.wasReleased()) { + //motorOn = false; + myDrive = 0; + setDisplay(myDrive, true); + } + + if (M5.BtnB.pressedFor(1000)) { + setDisplay(myDrive, true); + } + + if (M5.BtnC.wasReleased()) { + //motorOn = true; + myDrive = 0; + setDisplay(myDrive, true); + } + + if (M5.BtnC.pressedFor(1000)) { + setDisplay(myDrive, true); + } diff --git a/src/configload.h b/src/configload.h new file mode 100644 index 0000000..983ad57 --- /dev/null +++ b/src/configload.h @@ -0,0 +1,55 @@ + + +// Loads the configuration from a file +void loadConfiguration(const char *filename, Config &config) { + // Open file for reading + File file = SPIFFS.open(filename); + StaticJsonDocument<1024> doc; + DeserializationError error = deserializeJson(doc, file); + if (error) + Serial.println(F("Failed to read file, using default configuration")); + + strlcpy(config.cartname, // <- destination + doc["cartname"] | "TarantlCart", // <- source + sizeof(config.cartname)); // <- destination's capacity + + strlcpy(config.ssid, // <- destination + doc["ssid"] | "TarantlVR", // <- source + sizeof(config.ssid)); // <- destination's capacity + + strlcpy(config.passwd, // <- destination + doc["passwd"] | "chillfumml", // <- source + sizeof(config.passwd)); // <- destination's capacity + + JsonObject speedprofile_0 = doc["speedprofile"][0]; + + config.speed_max = speedprofile_0["speed_max"] | -100; + config.speed_min = speedprofile_0["speed_min"] | 100; + config.steer_max = speedprofile_0["steer_max"] | -100; + config.steer_min = speedprofile_0["steer_min"] | 100; + config.accel_min = speedprofile_0["accel_min"] | 200; + config.boost_max = speedprofile_0["boost"] | 100; + + + + file.close(); +} + +// Prints the content of a file to the Serial +void printFile(const char *filename) { + // Open file for reading + File file = SPIFFS.open(filename); + if (!file) { + Serial.println(F("Failed to read file")); + return; + } + + // Extract each characters by one by one + while (file.available()) { + Serial.print((char)file.read()); + } + Serial.println(); + + // Close the file + file.close(); +} \ No newline at end of file diff --git a/src/display.h b/src/display.h new file mode 100644 index 0000000..540ecc0 --- /dev/null +++ b/src/display.h @@ -0,0 +1,13 @@ +void setDisplay(int myDrive, boolean clear){ + if(clear){M5.Lcd.fillScreen(TFT_BLACK);} + M5.Lcd.setCursor(0, 140, 2); + M5.Lcd.setTextSize(1); + M5.Lcd.print("speed: "); M5.Lcd.println(myDrive); + M5.Lcd.print("MAX: "); M5.Lcd.println(config.speed_max); + M5.Lcd.print("MIN: "); M5.Lcd.println(config.speed_min); + M5.Lcd.print("ST-MAX: "); M5.Lcd.println(config.steer_max); + M5.Lcd.print("ST-MIN: "); M5.Lcd.println(config.steer_min); + M5.Lcd.print("accel: "); M5.Lcd.println(accel); + + //M5.Lcd.print("boost: "); M5.Lcd.println(boost); +} \ No newline at end of file diff --git a/src/hoverboard_telemetry.h b/src/hoverboard_telemetry.h new file mode 100644 index 0000000..d6fa6ec --- /dev/null +++ b/src/hoverboard_telemetry.h @@ -0,0 +1,69 @@ +// ########################## SEND ########################## +void Send(int16_t uSteer, int16_t uSpeed) +{ + // Create command + Command.start = (uint16_t)START_FRAME; + //Command.pitch = 0; + //Command.dpitch = 0; + Command.steer = (int16_t)uSteer; + Command.speed = (int16_t)uSpeed; + //Command.sensors = ; + Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed); + + // Write to Serial + HoverSerial.write((uint8_t *) &Command, sizeof(Command)); +} + +// ########################## RECEIVE ########################## +void Receive() +{ + if (HoverSerial.available()) { + incomingByte = HoverSerial.read(); + bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev; + } + else { + return; + } + + #ifdef DEBUG_RX + Serial.print(incomingByte); + return; + #endif + + if (bufStartFrame == START_FRAME) { + p = (byte *)&NewFeedback; + *p++ = incomingBytePrev; + *p++ = incomingByte; + idx = 2; + } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { + *p++ = incomingByte; + idx++; + } + + if (idx == sizeof(SerialFeedback)) { + uint16_t checksum; + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas + ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed); + + if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { + memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + + Serial.print("1: "); Serial.print(Feedback.cmd1); + Serial.print(" 2: "); Serial.print(Feedback.cmd2); + Serial.print(" 3: "); Serial.print(Feedback.speedR_meas); + Serial.print(" 4: "); Serial.print(Feedback.speedL_meas); + Serial.print(" 5: "); Serial.print(Feedback.batVoltage); + Serial.print(" 6: "); Serial.print(Feedback.boardTemp); + Serial.print(" 7: "); Serial.println(Feedback.cmdLed); + + //driveSpeed = 2*3,14*8,255*Feedback.speedR_meas*60/1000; + //driveSpeed = 3,11 * Feedback.speedL_meas; + driveSpeed = (float)Feedback.speedL_meas; + } else { + Serial.println("Non-valid data skipped"); + } + idx = 0; + } + + incomingBytePrev = incomingByte; +} \ No newline at end of file diff --git a/src/main.ino b/src/main.ino new file mode 100644 index 0000000..533ce4b --- /dev/null +++ b/src/main.ino @@ -0,0 +1,239 @@ +//#include +#include +#include "wii_i2c.h" +#include +#include "BLEDevice.h" + +#include +#include +#include +#include + +#include +#include "FS.h" +#include "SPIFFS.h" + +struct Config { + char cartname[64]; + char ssid[64]; + char passwd[64]; + int speed_max; + int speed_min; + int steer_max; + int steer_min; + int accel_min; + int boost_max; +}; + +struct Config2 { + char cartname[64]; + char ssid[64]; + char passwd[64]; + int speed_max; + int speed_min; + int steer_max; + int steer_min; + int accel_min; + int boost_max; +}; + +struct Config3 { + char cartname[64]; + char ssid[64]; + char passwd[64]; + int speed_max; + int speed_min; + int steer_max; + int steer_min; + int accel_min; + int boost_max; +}; + +const char *filename = "/config.json"; +Config config; +Config2 config2; +Config3 config3; + +const char* ssid = config.ssid; +const char* password = config.passwd; +AsyncWebServer server(80); + +#define TaskStackSize 5120 +//#include "lvgl.h" +//#include "blejoypad.h" +#define PIN_SDA 21 +#define PIN_SCL 22 +#define WII_I2C_PORT 0 +#define READ_TASK_CPU 0 +#define READ_DELAY 30 +static unsigned int controller_type; + +// ########################## DEFINES ########################## +#define HOVER_SERIAL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) +#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication +#define TIME_SEND 100 // [ms] Sending time interval + + +//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) +HardwareSerial &HoverSerial = Serial2; + +// Global variables +uint8_t idx = 0; // Index for new data pointer +uint16_t bufStartFrame; // Buffer Start Frame +byte *p; // Pointer declaration for the new received data +byte incomingByte; +byte incomingBytePrev; +boolean motorOn = false; +boolean triggerstate = false; +boolean switchState = false; +boolean useNunchuk = true; +boolean useOta = true; +float driveSpeed = 0; + +boolean triggerReleased = true; +int configNum = 0; + +rampInt upRamp; +rampInt downRamp; + +int leftRightCalibration = 0; +int forwardReverseCalibration = 0; + +int thresholdMovement = 100; + +int leftRightValue = 0; +int forwardReverseValue = 0; + +int OLDleftRightValue = 0; +int OLDforwardReverseValue = 0; + +int accel = config.accel_min; // Acceleration time [ms] +int safetyCool = 10; +int myDrive = 0; +int oldmyDrive = 0; + +typedef struct{ + uint16_t start; + //uint16_t dpitch; // sideboard usart + //uint16_t pitch; // sideboard usart + int16_t steer; + int16_t speed; + //uint16_t sensors; + // 0:OFF or 1:ON for switching input when dual input is used + // controltype; + // 00:FOC 10:Sinusoidal 01:Commutation + // controlmode; + // 00:Voltage 10:Speed 01:Torque + // fieldweakening; + // 0:off 1:on + uint16_t checksum; +} SerialCommand; +SerialCommand Command; + +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t batVoltage; + int16_t boardTemp; + uint16_t cmdLed; + uint16_t checksum; +} SerialFeedback; +SerialFeedback Feedback; +SerialFeedback NewFeedback; + +#include "configload.h" +#include "display.h" + +// ########################## SETUP ########################## +void setup() +{ + M5.begin(); + Serial.begin(SERIAL_BAUD); + HoverSerial.begin(HOVER_SERIAL_BAUD); + + while (!SPIFFS.begin()) { + Serial.println(F("Failed to initialize SPIFFS")); + /*bool formatted = SPIFFS.format(); + if(formatted){ + Serial.println("\n\nSuccess formatting"); + }else{ + Serial.println("\n\nError formatting"); + }*/ + delay(1000); + } + + Serial.println(F("Loading configuration...")); + loadConfiguration(filename, config); + + Serial.println(F("Print config file...")); + printFile(filename); + + if(useNunchuk){ + if (wii_i2c_init(WII_I2C_PORT, PIN_SDA, PIN_SCL) != 0) { + Serial.printf("Error initializing nunchuk"); + return; + } + + const unsigned char *ident = wii_i2c_read_ident(); + controller_type = wii_i2c_decode_ident(ident); + + if (wii_i2c_start_read_task(READ_TASK_CPU, READ_DELAY) != 0) { + Serial.printf("Error creating task to read controller state"); + return; + } + } + + if(useOta){ + #include + } + + setDisplay(myDrive, true); +} + +#include "hoverboard_telemetry.h" + +void accelerAte(int16_t start, int16_t target){ + upRamp.go(start); + upRamp.go(target, accel, LINEAR, ONCEFORWARD); +} + +void calibrateCenter(int16_t statex, int16_t statey){ + leftRightCalibration = statex; + forwardReverseCalibration = statey; +} + +// ########################## LOOP ########################## + +unsigned long iTimeSend = 0; + +void loop(void) +{ + M5.update(); + myDrive = upRamp.update(); + unsigned long timeNow = millis(); + + if(useNunchuk){ + #include "nunchuk.h" + } + + Receive(); + #include "buttons.h" + + if (iTimeSend > timeNow) return; + iTimeSend = timeNow + TIME_SEND; + if(motorOn == true){ + Send(leftRightValue, myDrive); + setDisplay(myDrive, true); + oldmyDrive = myDrive; + }else{ + myDrive = 0; + Send(0, 0); + setDisplay(myDrive, true); + } + +} + diff --git a/src/main_nunchuk.ino b/src/main_nunchuk.ino deleted file mode 100644 index 4abe514..0000000 --- a/src/main_nunchuk.ino +++ /dev/null @@ -1,361 +0,0 @@ -#include -#include -#include "wii_i2c.h" -#include -#include "BLEDevice.h" -#define TaskStackSize 5120 -//#include "lvgl.h" -//#include "blejoypad.h" - -#define PIN_SDA 21 -#define PIN_SCL 22 -#define WII_I2C_PORT 0 -#define READ_TASK_CPU 0 -#define READ_DELAY 30 -static unsigned int controller_type; - -// ########################## DEFINES ########################## -#define HOVER_SERIAL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) -#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) -#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication -#define TIME_SEND 100 // [ms] Sending time interval -#define SPEED_MAX - 300 // [-] Maximum speed -#define SPEED_MIN 300 // [-] Minimum speed -#define STEER_MAX 200 // [-] Maximum steer -#define STEER_MIN - 200 // [-] Minimum steer -#define ACCEL_MIN 200 -#define BOOST 100 // - -//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) -HardwareSerial &HoverSerial = Serial2; - -// Global variables -uint8_t idx = 0; // Index for new data pointer -uint16_t bufStartFrame; // Buffer Start Frame -byte *p; // Pointer declaration for the new received data -byte incomingByte; -byte incomingBytePrev; -boolean motorOn = false; -boolean triggerstate = false; -boolean switchState = false; -boolean useNunchuk = true; -boolean boost = false; -boolean triggerReleased = true; - -rampInt upRamp; -rampInt downRamp; - -int leftRightCalibration = 0; -int forwardReverseCalibration = 0; - -int thresholdMovement = 100; - -int leftRightValue = 0; -int forwardReverseValue = 0; - -int OLDleftRightValue = 0; -int OLDforwardReverseValue = 0; - -int accel = ACCEL_MIN; // Acceleration time [ms] -int safetyCool = 10; -int myDrive = 0; -int oldmyDrive = 0; - -typedef struct{ - uint16_t start; - //uint16_t dpitch; // sideboard usart - //uint16_t pitch; // sideboard usart - int16_t steer; - int16_t speed; - //uint16_t sensors; - // 0:OFF or 1:ON for switching input when dual input is used - // controltype; - // 00:FOC 10:Sinusoidal 01:Commutation - // controlmode; - // 00:Voltage 10:Speed 01:Torque - // fieldweakening; - // 0:off 1:on - uint16_t checksum; -} SerialCommand; -SerialCommand Command; - -typedef struct{ - uint16_t start; - int16_t cmd1; - int16_t cmd2; - int16_t speedR_meas; - int16_t speedL_meas; - int16_t batVoltage; - int16_t boardTemp; - uint16_t cmdLed; - uint16_t checksum; -} SerialFeedback; -SerialFeedback Feedback; -SerialFeedback NewFeedback; - -// ########################## SETUP ########################## -void setup() -{ - M5.begin(); - Serial.begin(SERIAL_BAUD); - Serial.println("Hoverboard Serial v1.0"); - - HoverSerial.begin(HOVER_SERIAL_BAUD); - - if(useNunchuk){ - if (wii_i2c_init(WII_I2C_PORT, PIN_SDA, PIN_SCL) != 0) { - Serial.printf("Error initializing nunchuk :("); - return; - } - - const unsigned char *ident = wii_i2c_read_ident(); - controller_type = wii_i2c_decode_ident(ident); - - if (wii_i2c_start_read_task(READ_TASK_CPU, READ_DELAY) != 0) { - Serial.printf("Error creating task to read controller state"); - return; - } - } - - setDisplay(myDrive, true); -} - -// ########################## SEND ########################## -void Send(int16_t uSteer, int16_t uSpeed) -{ - // Create command - Command.start = (uint16_t)START_FRAME; - //Command.pitch = 0; - //Command.dpitch = 0; - Command.steer = (int16_t)uSteer; - Command.speed = (int16_t)uSpeed; - //Command.sensors = ; - Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed); - - // Write to Serial - HoverSerial.write((uint8_t *) &Command, sizeof(Command)); -} - -// ########################## RECEIVE ########################## -void Receive() -{ - if (HoverSerial.available()) { - incomingByte = HoverSerial.read(); - bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev; - } - else { - return; - } - - #ifdef DEBUG_RX - Serial.print(incomingByte); - return; - #endif - - if (bufStartFrame == START_FRAME) { - p = (byte *)&NewFeedback; - *p++ = incomingBytePrev; - *p++ = incomingByte; - idx = 2; - } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { - *p++ = incomingByte; - idx++; - } - - if (idx == sizeof(SerialFeedback)) { - uint16_t checksum; - checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas - ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed); - - if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { - memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); - - Serial.print("1: "); Serial.print(Feedback.cmd1); - Serial.print(" 2: "); Serial.print(Feedback.cmd2); - Serial.print(" 3: "); Serial.print(Feedback.speedR_meas); - Serial.print(" 4: "); Serial.print(Feedback.speedL_meas); - Serial.print(" 5: "); Serial.print(Feedback.batVoltage); - Serial.print(" 6: "); Serial.print(Feedback.boardTemp); - Serial.print(" 7: "); Serial.println(Feedback.cmdLed); - } else { - Serial.println("Non-valid data skipped"); - } - idx = 0; - } - - incomingBytePrev = incomingByte; -} - -void accelerAte(int16_t start, int16_t target){ - upRamp.go(start); - upRamp.go(target, accel, LINEAR, ONCEFORWARD); -} - -void brAke(int16_t start, int16_t target){ - upRamp.go(start); - upRamp.go(target, accel, LINEAR, ONCEBACKWARD); -} - -void calibrateCenter(int16_t statex, int16_t statey){ - leftRightCalibration = statex; - forwardReverseCalibration = statey; -} - -// ########################## LOOP ########################## - -unsigned long iTimeSend = 0; - -void loop(void) -{ - M5.update(); - myDrive = upRamp.update(); - - unsigned long timeNow = millis(); - - if(useNunchuk){ - const unsigned char *data = wii_i2c_read_data_from_task(); - - if (data) { - OLDleftRightValue = leftRightValue; - OLDforwardReverseValue = forwardReverseValue; - - wii_i2c_nunchuk_state state; - wii_i2c_decode_nunchuk(data, &state); - - M5.Lcd.setCursor(10, 0, 2); - M5.Lcd.setTextSize(1); - M5.Lcd.print("x: "); M5.Lcd.println(state.x); - M5.Lcd.print("y: "); M5.Lcd.println(state.y); - M5.Lcd.print("switch: "); M5.Lcd.println((state.c) ? "pressed" : "not pressed"); - M5.Lcd.print("trigger: "); M5.Lcd.println((state.z) ? "pressed" : "not pressed"); - - if(state.c){ - motorOn = true; - calibrateCenter(state.x, state.y); - } - if(state.z){ - motorOn = false; - } - - leftRightValue = state.x; - forwardReverseValue = state.y; - triggerstate = state.z; - switchState = state.c; - - if( leftRightValue > -15 && leftRightValue < 15 ){ - leftRightValue = 0; - }else{ - leftRightValue = leftRightValue - leftRightCalibration; - } - - if(forwardReverseValue > -15 && forwardReverseValue < 15){ - forwardReverseValue = 0; - }else{ - forwardReverseValue = forwardReverseValue - forwardReverseCalibration; - } - - leftRightValue = map(leftRightValue, -100, 100, STEER_MIN, STEER_MAX); - forwardReverseValue = map(forwardReverseValue, -100, 100, SPEED_MIN, SPEED_MAX); - - M5.Lcd.print("x: "); M5.Lcd.println(leftRightValue); - M5.Lcd.print("y: "); M5.Lcd.println(forwardReverseValue); - - if(OLDforwardReverseValue > forwardReverseValue){ // brake - //brAke(forwardReverseValue, forwardReverseValue); - accelerAte(OLDforwardReverseValue, forwardReverseValue); - }else if(OLDforwardReverseValue < forwardReverseValue){ // accelerate - //if(forwardReverseValue > OLDforwardReverseValue +100){; - accelerAte(OLDforwardReverseValue, forwardReverseValue); - //} - } - - } - } - - Receive(); - - if (M5.BtnA.wasReleased()) { - if(triggerReleased){ - motorOn = true; - myDrive = 0; - upRamp.go(0); - upRamp.go(SPEED_MIN, accel, LINEAR, ONCEFORWARD); - }else{ - triggerReleased = true; - } - setDisplay(myDrive, true); - } - - if (M5.BtnA.pressedFor(1000)) { - boost = !boost; - triggerReleased = false; - setDisplay(myDrive, true); - } - - if (M5.BtnB.wasReleased()) { - if(triggerReleased){ - motorOn = !motorOn; - accel = accel -100; - if(accel < 500){accel = 500;} - myDrive = 0; - upRamp.go(0); - }else{ - triggerReleased = true; - } - setDisplay(myDrive, true); - } - - if (M5.BtnB.pressedFor(1000)) { - accel = ACCEL_MIN; - triggerReleased = false; - setDisplay(myDrive, true); - } - - if (M5.BtnC.wasReleased()) { - if(triggerReleased){ - motorOn = true; - myDrive = 0; - upRamp.go(0); - if(boost){ - upRamp.go(SPEED_MAX + BOOST, accel, LINEAR, ONCEFORWARD); - }else{ - upRamp.go(SPEED_MAX, accel, LINEAR, ONCEFORWARD); - } - }else{ - triggerReleased = true; - } - setDisplay(myDrive, true); - } - - if (M5.BtnC.pressedFor(1000)) { - accel = 500; - triggerReleased = false; - setDisplay(myDrive, true); - } - - - if (iTimeSend > timeNow) return; - iTimeSend = timeNow + TIME_SEND; - if(motorOn == true){ - Send(leftRightValue, myDrive); - setDisplay(myDrive, true); - oldmyDrive = myDrive; - }else{ - Send(0, 0); - setDisplay(myDrive, true); - } - -} - -void setDisplay(int myDrive, boolean clear){ - if(clear){M5.Lcd.fillScreen(TFT_BLACK);} - M5.Lcd.setCursor(10, 100, 2); - M5.Lcd.setTextSize(1); - M5.Lcd.print("speed: "); M5.Lcd.println(myDrive); - M5.Lcd.print("MAX: "); M5.Lcd.println(SPEED_MAX); - M5.Lcd.print("MIN: "); M5.Lcd.println(SPEED_MIN); - M5.Lcd.print("ST-MAX: "); M5.Lcd.println(STEER_MAX); - M5.Lcd.print("ST-MIN: "); M5.Lcd.println(STEER_MIN); - M5.Lcd.print("accel: "); M5.Lcd.println(accel); - //M5.Lcd.print("boost: "); M5.Lcd.println(boost); -} \ No newline at end of file diff --git a/src/mywifi.h b/src/mywifi.h new file mode 100644 index 0000000..aa80fcb --- /dev/null +++ b/src/mywifi.h @@ -0,0 +1,23 @@ + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); + Serial.println(""); + + // Wait for connection + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); + } + Serial.println(""); + Serial.print("Connected to "); + Serial.println(ssid); + Serial.print("IP address: "); + Serial.println(WiFi.localIP()); + + server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) { + request->send(200, "text/plain", config.cartname); + }); + + AsyncElegantOTA.setID(config.cartname); + AsyncElegantOTA.begin(&server, "Super", "Tarantl"); + server.begin(); + Serial.println("HTTP server started"); \ No newline at end of file diff --git a/src/nunchuk.h b/src/nunchuk.h new file mode 100644 index 0000000..70e0b66 --- /dev/null +++ b/src/nunchuk.h @@ -0,0 +1,70 @@ + const unsigned char *data = wii_i2c_read_data_from_task(); + + if(data) { + OLDleftRightValue = leftRightValue; + OLDforwardReverseValue = forwardReverseValue; + + wii_i2c_nunchuk_state state; + wii_i2c_decode_nunchuk(data, &state); + + M5.Lcd.setCursor(0, 0, 2); + M5.Lcd.setTextSize(1); + M5.Lcd.print("x: "); M5.Lcd.println(state.x); + M5.Lcd.print("y: "); M5.Lcd.println(state.y); + M5.Lcd.print("switches: "); M5.Lcd.print((state.c) ? "C pressed" : "C not pressed "); M5.Lcd.println((state.z) ? "Z pressed" : "Z not pressed"); + M5.Lcd.print("Motor: "); M5.Lcd.println((motorOn) ? "true" : "false"); + M5.Lcd.print("Config: "); M5.Lcd.println(configNum); + M5.Lcd.print("rpm: "); M5.Lcd.println(driveSpeed); + + float speedFactor = 2.0 * 3.14 * 0.08255 * 60 /1000; + M5.Lcd.print("fac: "); M5.Lcd.println(speedFactor); + M5.Lcd.print("km/h: "); M5.Lcd.println(driveSpeed * speedFactor); + + if(state.c){ + motorOn = !motorOn; + calibrateCenter(state.x, state.y); + delay(200); + } + if(state.z){ + configNum = configNum+1; + if(configNum > 4){ + configNum = 0; + } + delay(200); + } + + leftRightValue = state.x; + forwardReverseValue = state.y; + triggerstate = state.z; + switchState = state.c; + + if( leftRightValue > -15 && leftRightValue < 15 ){ + leftRightValue = 0; + //}else if(leftRightValue < config.steer_max && leftRightValue > config.steer_min){ + //leftRightValue = 0; + }else{ + leftRightValue = leftRightValue - leftRightCalibration; + } + + if(forwardReverseValue > -15 && forwardReverseValue < 15){ + forwardReverseValue = 0; + }else if(forwardReverseValue < config.speed_max+(config.boost_max*configNum) && forwardReverseValue > config.speed_min+(config.boost_max*configNum)){ + forwardReverseValue = 0; + }else{ + forwardReverseValue = forwardReverseValue - forwardReverseCalibration; + } + + leftRightValue = map(leftRightValue, -100, 100, config.steer_min-(config.boost_max*configNum), config.steer_max+(config.boost_max*configNum)); + forwardReverseValue = map(forwardReverseValue, -100, 100, config.speed_min+(config.boost_max*configNum), config.speed_max-(config.boost_max*configNum)); + + M5.Lcd.print("x: "); M5.Lcd.println(leftRightValue); + M5.Lcd.print("y: "); M5.Lcd.println(forwardReverseValue); + + + if(OLDforwardReverseValue > forwardReverseValue){ // brake + accelerAte(OLDforwardReverseValue, forwardReverseValue); + }else if(OLDforwardReverseValue < forwardReverseValue){ + accelerAte(OLDforwardReverseValue, forwardReverseValue); + } + + } \ No newline at end of file