Bug: IMU Data Not Updating

ArduinoController/ArduinoController.ino

@@ -10,43 +10,41 @@
 #include "StartController.h"
 #include "PinJSON.h"
 
-const uint8_t KILLPIN = 50; // Needs to remain global (therefore outside of JSON) for lambda interrupt
-const uint8_t KILL_ADDR = 10;
 const uint32_t CONTROLLER_CNT = 13u;
 class IController* controllers[CONTROLLER_CNT];
 
 void setup() {
   Serial.begin(115200);
-  Serial.print('R');
-  DynamicJsonBuffer buffer;
-  auto json = buffer.parse(PinJSON::json);
+  Serial1.begin(9600);
+  SerialTools::writeString("Ready!", 6);
 
-  controllers[0] = new ThrustController(json["thrust"]["left_forward"].as<uint8_t>(), 50);
-  controllers[1] = new ThrustController(json["thrust"]["right_forward"].as<uint8_t>(), 50);
-  controllers[2] = new ThrustController(json["thrust"]["left_strafe"].as<uint8_t>(), 50);
-  controllers[3] = new ThrustController(json["thrust"]["right_strafe"].as<uint8_t>(), 50);
-  controllers[4] = new ThrustController(json["thrust"]["front_dive"].as<uint8_t>());
-  controllers[5] = new ThrustController(json["thrust"]["back_dive"].as<uint8_t>());
-  controllers[6] = new EscController(json["esc"].asObject());
-  controllers[7] = new LedController(json["led"].asObject());
-  controllers[8] = new PingController();
-  controllers[9] = new LightController(json["light"]["lights"].as<uint8_t>());
-  controllers[11]= new VoltageController(json["voltage"]["volt"].as<uint8_t>());
-  controllers[12]= new StartController();
-  controllers[KILL_ADDR]= new KillSwitchController(controllers, CONTROLLER_CNT, KILL_ADDR);
+  controllers[0]= new KillSwitchController(controllers, CONTROLLER_CNT);
   attachInterrupt(
-    digitalPinToInterrupt(KILLPIN), 
-    [](){((KillSwitchController*)controllers[KILL_ADDR])->isr(KILLPIN);},
+    digitalPinToInterrupt(KILLSWITCH_PIN), 
+    [](){((KillSwitchController*)controllers[0])->isr(KILLSWITCH_PIN);},
     CHANGE
   );
- while((!Serial.available())||(Serial.read()==0));
+  controllers[1]  = new ThrustController(MOVE_PIN);
+  controllers[2]  = new ThrustController(STRAFE_PIN);
+  controllers[3]  = new ThrustController(DIVE_PIN);
+  controllers[4]  = new ThrustController(YAW_PIN);
+  controllers[5]  = new ThrustController(PITCH_PIN);
+  controllers[6]  = new ThrustController(ROLL_PIN);
+  controllers[7]  = new EscController();
+  controllers[8]  = new LedController();
+  controllers[9]  = new PingController();
+  controllers[10] = new LightController();
+  controllers[11] = new VoltageController();
+  controllers[12] = new StartController();
+
+  while((!Serial.available())||(Serial.read()==0));
 }
 
 void loop() {
-  if(Serial.available()) {
-    uint8_t controllerNumber = Serial.read();
-    if(controllerNumber < CONTROLLER_CNT)
-      // only execute if a command exists
-      controllers[controllerNumber]->execute();
+  uint8_t controllerIndex = SerialTools::readByte();
+  if(controllerIndex < CONTROLLER_CNT) {
+    // only execute if a command exists
+    DMSG("Controller: "); DMSGN(controllerIndex);
+    controllers[controllerIndex]->execute();
   }
-}
+}

ArduinoController/Debugger.h

@@ -0,0 +1,8 @@
+#ifndef _DEBUGGER_
+#define _DEBUGGER_
+
+#define DMSG(x) Serial1.print(x)
+#define DHEX(x) Serial1.print(x,HEX)
+#define DMSGN(x) Serial1.println(x)
+
+#endif

ArduinoController/EscController.h

@@ -1,17 +1,23 @@
 #include "IController.h"
 
+#define GPIO_CNT 6u
+
 class EscController : public IController {
 
 private:
-
-  uint8_t pinCount;
-
-  uint8_t* pins_;
-
-  void assignPins_(JsonArray& gpioPins) {
-    pins_ = new uint8_t[pinCount];
-    for(int i = 0; i < pinCount; i++) {
-      pins_[i] = gpioPins[1].as<uint8_t>();
+  uint8_t GPIO_PINS[GPIO_CNT] = {
+    ESC_S1_PIN,
+    ESC_S2_PIN,
+    ESC_S3_PIN,
+    ESC_S4_PIN,
+    ESC_S5_PIN,
+    ESC_S6_PIN
+  };
+public:
+  EscController() {
+    for(uint32_t i = 0; i < GPIO_CNT; i++) {
+      pinMode(GPIO_PINS[i], OUTPUT);
+      digitalWrite(GPIO_PINS[i], HIGH);
     }
   }
 
@@ -31,16 +37,17 @@ class EscController : public IController {
   }
 
   void execute() {
-    while(!Serial.available());
-    uint8_t toggle = Serial.read();
-    for(int i = 0; i < pinCount; i++) {
-      digitalWrite(pins_[i], !toggle);
+    uint8_t toggle = SerialTools::readByte();
+    for(uint32_t i = 0; i < GPIO_CNT; i++) {
+      digitalWrite(GPIO_PINS[i], !toggle);
     }
   }
 
   void kill() {
-    for(int i = 0; i < pinCount; i++) {
-      digitalWrite(pins_[i], HIGH);
+    for(uint32_t i = 0; i < GPIO_CNT; i++) {
+      digitalWrite(GPIO_PINS[i], HIGH);
     }
   }
-};
+};
+
+#undef GPIO_CNT

ArduinoController/IController.h

@@ -2,6 +2,8 @@
 #define ITHRUSTER_H
 
 #include "Arduino.h"
+#include "SerialTools.h"
+#include "PinMap.h"
 
 class IController {
  public:

ArduinoController/KillSwitchController.h

@@ -4,26 +4,24 @@ class KillSwitchController : public IController {
 private:
   volatile int active;
   IController ** list;
-  int count, self;
-  const int STAT_LED = 22;
+  const int ACTIVE   = 0x1;
+  const int INACTIVE = 0x0;
+  int count;
 
 public:
-  KillSwitchController(IController ** _list_, int _count_, int _self_) {
+  KillSwitchController(IController ** _list_, int _count_) {
     list = _list_;
     count = _count_;
-    self = _self_;
-    pinMode(STAT_LED, OUTPUT);
+    pinMode(LED_STAT_PIN, OUTPUT);
   }
 
   void execute() {
-    Serial.println(active?'1':'0');
+    SerialTools::writeByte(active ? ACTIVE : INACTIVE);
   }
 
   void kill() {
-    for(int i = 0; i<count; i++) {
-      if(i != self)
-        list[i]->kill();
-    }
+    for(int i = 1; i < count; i++)
+      list[i]->kill();
   }
 
   void isr(int interrupt) {
@@ -33,7 +31,7 @@ class KillSwitchController : public IController {
     active = !digitalRead(interrupt);
     if(active)
       kill();
-    digitalWrite(STAT_LED, active);
+    digitalWrite(LED_STAT_PIN, active);
     interrupts();
   }
 };

ArduinoController/LedController.h

@@ -2,23 +2,20 @@
 
 class LedController : public IController {
 private:
-  uint8_t green_;
-  uint8_t white_;
 public:
-  LedController(JsonObject& pins) : green_(pins["green"]), white_(pins["white"]) {
-    pinMode(white_, OUTPUT);
-    digitalWrite(white_, LOW);
+  LedController() {
+    pinMode(LED_CTRL_PIN, OUTPUT);
+    digitalWrite(LED_CTRL_PIN, LOW);
   }
   void execute() {
     for(int i = 0; i < 2; i++) {
-      digitalWrite(white_, HIGH);
+      digitalWrite(LED_CTRL_PIN, HIGH);
       delay(250);
-      digitalWrite(white_, LOW);
+      digitalWrite(LED_CTRL_PIN, LOW);
       delay(250);
     }
   }
   void kill() {
-    digitalWrite(green_, LOW);
-    digitalWrite(white_, LOW);
+    digitalWrite(LED_CTRL_PIN, LOW);
   }
-};
+};

ArduinoController/LightController.h

@@ -1,18 +1,13 @@
 #include "IController.h"
 
 class LightController : public IController {
-private:
-  uint8_t lights_ = 45;
 public:
-  LightController(uint8_t lights) : lights_(lights) {
-    pinMode(lights_, OUTPUT);
-    digitalWrite(lights_, HIGH);
+  LightController() {
+    pinMode(LIGHTS_PIN, OUTPUT);
+    digitalWrite(LIGHTS_PIN, HIGH);
   }
   void execute() {
-    while(!Serial.available());
-    digitalWrite(lights_, !Serial.read());
+    digitalWrite(LIGHTS_PIN, !SerialTools::readByte());
   }
-  void kill() {
-    //digitalWrite(LIGHTS, HIGH);
-  }
-};
+  void kill() { }
+};

ArduinoController/PinMap.h

@@ -0,0 +1,28 @@
+#ifndef PIN_MAP_H
+#define PIN_MAP_H
+
+#define KILLSWITCH_PIN  50
+
+#define LED_STAT_PIN    22
+#define LED_CTRL_PIN    52
+
+#define LIGHTS_PIN      45
+
+#define MOVE_PIN        13
+#define STRAFE_PIN      12
+#define DIVE_PIN        11
+
+#define YAW_PIN         9
+#define PITCH_PIN       8
+#define ROLL_PIN        7
+
+#define ESC_S1_PIN      36
+#define ESC_S2_PIN      39
+#define ESC_S3_PIN      41
+#define ESC_S4_PIN      47
+#define ESC_S5_PIN      49
+#define ESC_S6_PIN      53
+
+#define VOLT_PIN        10
+
+#endif

ArduinoController/PingController.h

@@ -3,12 +3,7 @@
 class PingController : public IController {
 public:
   void execute() {
-    if(Serial.available())
-    {
-      Serial.print(Serial.read());
-      Serial.print(" ");
-    }
-    Serial.println("I'm Here!");
+    SerialTools::writeString("I'm Here!", 9);
   }
   void kill() { }
 };