Initial worked

include/tests/carClassTest.h

@@ -0,0 +1,44 @@
+#include <PinChangeInterrupt.h>
+#include "Car.h"
+#include "config.h"
+
+Matrix<3> desiredCarVelocity;
+Matrix<3> feedbackCarPose;
+
+unsigned long wheelPeriod = UPDATE_STATE_DT_MS; // millis
+Car car(WHEELBASE_LENGTH / 2, WHEELBASE_WIDTH / 2, OMNIWHEEL_DIAMETER / 2, UPDATE_STATE_DT_MS, &desiredCarVelocity, &feedbackCarPose);
+void updateW1A() { car.wheels[0]->triggerA(); }
+void updateW1B() { car.wheels[0]->triggerB(); }
+void updateW2A() { car.wheels[1]->triggerA(); }
+void updateW2B() { car.wheels[1]->triggerB(); }
+void updateW3A() { car.wheels[2]->triggerA(); }
+void updateW3B() { car.wheels[2]->triggerB(); }
+void updateW4A() { car.wheels[3]->triggerA(); }
+void updateW4B() { car.wheels[3]->triggerB(); }
+
+void setup()
+{
+    attachPCINT(digitalPinToPCINT(car.wheels[0]->getEncPinA()), updateW1A, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[0]->getEncPinB()), updateW1B, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[1]->getEncPinA()), updateW2A, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[1]->getEncPinB()), updateW2B, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[2]->getEncPinA()), updateW3A, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[2]->getEncPinB()), updateW3B, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[3]->getEncPinA()), updateW4A, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[3]->getEncPinB()), updateW4B, RISING);
+
+    Serial.begin(38400);
+    Serial.setTimeout(100);
+}
+
+void loop()
+{
+    //                     t  x  y
+    car.setDesiredVelocity(0, 0.1, 0); // 5, 1, 1
+    auto t1 = millis();
+    car.update();
+    auto t2 = millis();
+    Serial.print("Time for car update: ");
+    Serial.println(t2-t1);
+    // delay(1000);
+}

include/tests/encoderClassTest.h

@@ -0,0 +1,29 @@
+#include <PinChangeInterrupt.h>
+#include "Encoder.h"
+#include "config.h"
+
+int pinA = ENC_MOTOR2_PINA;
+int pinB = ENC_MOTOR2_PINB;
+Encoder encoder(pinA, pinB, false);
+void triggerA() { encoder.triggerA(); }
+void triggerB() { encoder.triggerB(); }
+
+void setup() {
+    encoder.reset();
+    if(digitalPinToInterrupt(pinA) == -1){
+        attachPCINT(digitalPinToPCINT(pinA), triggerA, RISING);
+    } else{
+        attachInterrupt(digitalPinToInterrupt(pinA), triggerA, RISING);
+    }
+    if(digitalPinToInterrupt(pinB) == -1){
+        attachPCINT(digitalPinToPCINT(pinB), triggerB, RISING);
+    } else{
+        attachInterrupt(digitalPinToInterrupt(pinB), triggerB, RISING);
+    }
+
+    Serial.begin(38400);
+}
+
+void loop() {
+    Serial.println("Position " + (String)encoder.getAngle()+" Ticks " + (String)encoder.getTicks());
+}

include/encoderTest.h → include/tests/encoderTest.h

@@ -1,8 +1,9 @@
 #include <PinChangeInterrupt.h>
+#include "config.h"
 // #include "Arduino.h"
 
-const int pinA = 18;
-const int pinB = 19;
+const int pinA = ENC_MOTOR3_PINA;
+const int pinB = ENC_MOTOR3_PINB;
 
 volatile long position = 0;
 
@@ -33,12 +34,15 @@ void triggerB() {
 void setup() {
     pinMode(pinA, INPUT_PULLUP);
     pinMode(pinB, INPUT_PULLUP);
-    attachInterrupt(digitalPinToInterrupt(pinA), triggerA, RISING);
-    attachInterrupt(digitalPinToInterrupt(pinB), triggerB, RISING);
-    // attachPCINT(digitalPinToPCINT(pinA), triggerA, RISING);
-    // attachPCINT(digitalPinToPCINT(pinB), triggerB, RISING);
 
-    Serial.begin(115200);
+    // attachInterrupt(digitalPinToInterrupt(pinA), triggerA, RISING);
+    // attachInterrupt(digitalPinToInterrupt(pinB), triggerB, RISING);
+
+    // Use program interrupt
+    attachPCINT(digitalPinToPCINT(pinA), triggerA, RISING);
+    attachPCINT(digitalPinToPCINT(pinB), triggerB, RISING);
+
+    Serial.begin(38400);
 }
 
 void loop() {}

include/tests/finalSystem.h

@@ -0,0 +1,59 @@
+#include <PinChangeInterrupt.h>
+#include <Thread.h>
+#include "Car.h"
+#include "SerialTransceiver.h"
+#include "config.h"
+
+Matrix<3> desiredCarVelocity;
+Matrix<3> feedbackCarPose;
+
+Car car(WHEELBASE_LENGTH / 2, WHEELBASE_WIDTH / 2, OMNIWHEEL_DIAMETER / 2, UPDATE_STATE_DT_MS, &desiredCarVelocity, &feedbackCarPose);
+void updateW1A() { car.wheels[0]->triggerA(); }
+void updateW1B() { car.wheels[0]->triggerB(); }
+void updateW2A() { car.wheels[1]->triggerA(); }
+void updateW2B() { car.wheels[1]->triggerB(); }
+void updateW3A() { car.wheels[2]->triggerA(); }
+void updateW3B() { car.wheels[2]->triggerB(); }
+void updateW4A() { car.wheels[3]->triggerA(); }
+void updateW4B() { car.wheels[3]->triggerB(); }
+
+SerialTransceiver transceiver(&desiredCarVelocity, &feedbackCarPose);
+
+Thread carThread = Thread();
+Thread serialThread = Thread();
+
+void updateCar() { car.update(); }
+void talkSerial() { transceiver.talk(); }
+
+void setup()
+{
+    attachPCINT(digitalPinToPCINT(car.wheels[0]->getEncPinA()), updateW1A, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[0]->getEncPinB()), updateW1B, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[1]->getEncPinA()), updateW2A, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[1]->getEncPinB()), updateW2B, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[2]->getEncPinA()), updateW3A, RISING);
+    attachPCINT(digitalPinToPCINT(car.wheels[2]->getEncPinB()), updateW3B, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[3]->getEncPinA()), updateW4A, RISING);
+    attachInterrupt(digitalPinToInterrupt(car.wheels[3]->getEncPinB()), updateW4B, RISING);
+
+    desiredCarVelocity.Fill(0);
+    feedbackCarPose.Fill(0);
+
+    Serial.begin(38400);
+    Serial.setTimeout(100);
+
+    carThread.onRun(updateCar);
+    serialThread.onRun(talkSerial);
+}
+
+void loop()
+{
+    if (carThread.shouldRun())
+    {
+        carThread.run();
+    }
+    if (serialThread.shouldRun())
+    {
+        serialThread.run();
+    }
+}

include/tests/motorClassTest.h

@@ -0,0 +1,19 @@
+#include <Arduino.h>
+#include "Motor.h"
+
+unsigned long period = 500;
+int voltageValues[] = {50, 0, 60, 0, 69, 0, 70, 0, 71, 0, 80, 0, 200, 0, -255, 255, 0};
+int arrayLength = sizeof(voltageValues) / sizeof(int);
+Motor motor(1);
+
+void setup() {
+    Serial.begin(38400);
+}
+
+void loop() {
+    unsigned long step = millis() / period;
+    if (step < arrayLength) {
+        motor.setValue(voltageValues[step]);
+        Serial.println("Value = " + (String)voltageValues[step]);
+    }
+}