Changing filename, calibrating output values, adding proper failsafe

src/receiver.cpp

@@ -0,0 +1,114 @@
+// ESPNOWSkate Receiver by Lukas Bachschwell this device SLAVE =D
+#include <Servo.h>
+#include "Arduino.h"
+#include "SSD1306.h"
+#include <esp_now.h>
+#include <WiFi.h>
+
+#define esc1pin 15
+#define esc2pin 13
+#define fallbackpin 36
+
+
+Servo esc1;
+Servo esc2;
+SSD1306 display(0x3c, 5, 4);
+
+
+//#define pairingMode
+#define CONNECTION_TIMEOUT 300
+#define CHANNEL 1
+long lastPacket = 0;
+
+bool isConnected = false;
+
+// ESPNOW Functions ############################
+// config AP
+void configDeviceAP(bool hidden) {
+  bool result = WiFi.softAP("ESK8", "ESK8_Password+vD8z2YAvoDBW?Zx", CHANNEL, hidden);
+  if (!result) {
+    Serial.println("AP Config failed.");
+  } else {
+    Serial.println("AP Config Success. Broadcasting with AP: " + String("ESK8"));
+  }
+}
+
+void writeServos(uint8_t firstServo, uint8_t secondServo) {
+  esc1.write(firstServo);
+  esc2.write(secondServo);
+}
+
+// callback when data is recv from Master
+void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
+  char macStr[18];
+  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
+           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
+  Serial.print("Last Packet Recv from: "); Serial.println(macStr);
+  uint8_t recData[2];
+  memcpy(recData, data, data_len);
+  Serial.print("Last Packet Recv Data: "); Serial.println(recData[0]); Serial.print(" "); Serial.print(recData[1]); Serial.print(" len:");  Serial.println(data_len);
+
+  lastPacket = millis();
+  isConnected = true;
+  // Could check mac here for some security
+  writeServos(recData[0], recData[1]);
+
+  display.clear();
+  char buf[25];
+  sprintf(buf, "1: %i | 2: %i", recData[0], recData[1]);
+  display.drawString(2, 0, buf);
+  display.display();
+}
+
+// end ESPNOW functions
+
+void setup() {
+  Serial.begin(115200);
+  Serial.println("ESPNowSkate Receiver");
+
+  display.init();
+  display.flipScreenVertically();
+  display.setFont(ArialMT_Plain_16);
+
+  // Init escs, min and max value similar as Traxxas TQI 1100, 1900
+  // chanel, minAngel, maxAngel, minPulseWidth, maxPulseWidth
+  esc1.attach(esc1pin, 0, 0, 180, 1100, 1900);
+  esc2.attach(esc2pin, 1, 0, 180, 1100, 1900);
+
+  //Set device in AP mode to begin with
+  WiFi.mode(WIFI_AP);
+  // configure device AP mode
+  #ifdef pairingMode
+  configDeviceAP(false);
+  #else
+  configDeviceAP(true);
+  #endif
+
+  Serial.print("AP MAC: "); Serial.println(WiFi.softAPmacAddress());
+
+  // Init ESPNow
+  if (esp_now_init() == ESP_OK) {
+    Serial.println("ESPNow Init Success");
+  }
+  else {
+    Serial.println("ESPNow Init Failed");
+    ESP.restart();
+  }
+  // Once ESPNow is successfully Init, we will register for recv CB to
+  // get recv packer info.
+  esp_now_register_recv_cb(OnDataRecv);
+}
+
+
+void loop() {
+  if(millis() - lastPacket > CONNECTION_TIMEOUT ) {
+    isConnected = false;
+    int failsafeValue = map(analogRead(fallbackpin), 0, 4095, 0, 180);
+    writeServos(failsafeValue, failsafeValue);
+    display.clear();
+    char buf[25];
+    sprintf(buf, "FAIL: %i", failsafeValue);
+    display.drawString(2, 0, buf);
+    display.display();
+  }
+}