Added serial communication control

lib/Car/Car.cpp

@@ -1,11 +1,11 @@
 #include "Car.h"
 
-Car::Car(int mPin1, int mPin2, int mPin3, int mPin4, int ePin1, int ePin2, int ePin3, int ePin4, int intervalMillis) {
+Car::Car(int encoderPins[4], int intervalMillis) {
     int updateShift = intervalMillis / 4;
-    w1 = new Wheel(mPin1, ePin1, intervalMillis, 0);
-    w2 = new Wheel(mPin2, ePin2, intervalMillis, updateShift);
-    w3 = new Wheel(mPin3, ePin3, intervalMillis, updateShift * 2);
-    w4 = new Wheel(mPin4, ePin4, intervalMillis, updateShift * 3);
+    w1 = new Wheel(1, encoderPins[0], intervalMillis, 0);
+    w2 = new Wheel(2, encoderPins[1], intervalMillis, updateShift);
+    w3 = new Wheel(3, encoderPins[2], intervalMillis, updateShift * 2);
+    w4 = new Wheel(4, encoderPins[3], intervalMillis, updateShift * 3);
 }
 Car::~Car() {
     delete w1;
@@ -14,11 +14,27 @@ Car::~Car() {
     delete w4;
 }
 
-void Car::runForward(float rpm) {
-    w1->reachVelocity(rpm);
-    w2->reachVelocity(rpm);
-    w3->reachVelocity(rpm);
-    w4->reachVelocity(rpm);
+float* Car::getFeedbackRpms() { return feedbackRpms; }
+
+void Car::setDesiredRpms(float values[4]) {
+    desiredRpms[0] = values[0];
+    desiredRpms[1] = values[1];
+    desiredRpms[2] = values[2];
+    desiredRpms[3] = values[3];
+}
+
+void Car::updateFeedbackRpms() {
+    feedbackRpms[0] = w1->getRPM();
+    feedbackRpms[1] = w2->getRPM();
+    feedbackRpms[2] = w3->getRPM();
+    feedbackRpms[3] = w4->getRPM();
+}
+
+void Car::reachDesiredRpms() {
+    w1->reachVelocity(desiredRpms[0]);
+    w2->reachVelocity(desiredRpms[1]);
+    w3->reachVelocity(desiredRpms[2]);
+    w4->reachVelocity(desiredRpms[3]);
 }
 
 int Car::getEncPin1() { return w1->getEncPin(); }
@@ -30,8 +46,3 @@ void Car::incEnc1() { w1->incEnc(); }
 void Car::incEnc2() { w2->incEnc(); }
 void Car::incEnc3() { w3->incEnc(); }
 void Car::incEnc4() { w4->incEnc(); }
-
-float Car::getRPM1() { return w1->getRPM(); }
-float Car::getRPM2() { return w2->getRPM(); }
-float Car::getRPM3() { return w3->getRPM(); }
-float Car::getRPM4() { return w4->getRPM(); }

lib/Car/Car.h

@@ -3,15 +3,20 @@
 class Car
 {
 private:
+    float desiredRpms[4] = {}; // init with zeros
+    float feedbackRpms[4] = {}; // init with zeros
     Wheel *w1;
     Wheel *w2;
     Wheel *w3;
     Wheel *w4;
 
 public:
-    Car(int mPin1, int mPin2, int mPin3, int mPin4, int ePin1, int ePin2, int ePin3, int ePin4, int intervalMillis);
+    Car(int encoderPins[4], int intervalMillis);
     ~Car();
-    void runForward(float rpm);
+    float* getFeedbackRpms();
+    void setDesiredRpms(float[4]);
+    void updateFeedbackRpms();
+    void reachDesiredRpms();
 
     int getEncPin1();
     int getEncPin2();
@@ -22,9 +27,4 @@ class Car
     void incEnc2();
     void incEnc3();
     void incEnc4();
-
-    float getRPM1();
-    float getRPM2();
-    float getRPM3();
-    float getRPM4();
 };

lib/SerialTalker/SerialTalker.cpp

@@ -0,0 +1,27 @@
+#include "SerialTalker.h"
+
+SerialTalker::SerialTalker() {}
+
+float* SerialTalker::rx() {
+    Serial.readBytesUntil('\n', buffer, 17);
+    for (int i = 0; i < 4; i++) {
+        n1.b[i] = buffer[i];
+        n2.b[i] = buffer[i + 4];
+        n3.b[i] = buffer[i + 8];
+        n4.b[i] = buffer[i + 12];
+    }
+    rpms[0] = n1.f;
+    rpms[1] = n2.f;
+    rpms[2] = n3.f;
+    rpms[3] = n4.f;
+    return rpms;
+}
+
+void SerialTalker::tx(float values[4]) {
+    rpms[0] = values[0];
+    rpms[1] = values[1];
+    rpms[2] = values[2];
+    rpms[3] = values[3];
+    Serial.write((byte*)&rpms, sizeof(float)*4);
+    Serial.write('\n');
+}

lib/SerialTalker/SerialTalker.h

@@ -0,0 +1,18 @@
+#include <Arduino.h>
+
+class SerialTalker
+{
+private:
+    byte buffer[17];
+    union { float f; byte b[4]; } n1;
+    union { float f; byte b[4]; } n2;
+    union { float f; byte b[4]; } n3;
+    union { float f; byte b[4]; } n4;
+    float rpms[4];
+
+public:
+    SerialTalker();
+    float* rx();
+    void tx(float rpms[4]);
+
+};

src/main.cpp

@@ -1,113 +1,31 @@
 #include <PinChangeInterrupt.h>
 // #include "Wheel.h"
 #include "Car.h"
+#include "SerialTalker.h"
 
-int intervalMillis = 40; // millis
-Car car(1, 2, 3, 4, 2, 13, 18, 19, intervalMillis);
-// Wheel w1(1, 2,  intervalMillis);
-// Wheel w2(2, 13, intervalMillis);
-// Wheel w3(3, 18, intervalMillis);
-// Wheel w4(4, 19, intervalMillis);
+SerialTalker talker;
+int intervalMillis = 40;
+int encoderPins[4] = {2, 13, 18, 19};
+Car car(encoderPins, intervalMillis);
 void updateW1() { car.incEnc1(); }
 void updateW2() { car.incEnc2(); }
 void updateW3() { car.incEnc3(); }
 void updateW4() { car.incEnc4(); }
-// void updateW1() { w1.incEnc(); }
-// void updateW2() { w2.incEnc(); }
-// void updateW3() { w3.incEnc(); }
-// void updateW4() { w4.incEnc(); }
-int vel = 0;
-int u = 0;
-
-unsigned long now = 0;
-unsigned long prev = 0;
-unsigned long timer = 5000;
-bool state = false;
 
 void setup() {
   attachInterrupt(digitalPinToInterrupt(car.getEncPin1()), updateW1, RISING);
   attachPCINT(digitalPinToPCINT(car.getEncPin2()), updateW2, RISING);
   attachInterrupt(digitalPinToInterrupt(car.getEncPin3()), updateW3, RISING);
   attachInterrupt(digitalPinToInterrupt(car.getEncPin4()), updateW4, RISING);
-  // attachInterrupt(digitalPinToInterrupt(w1.getEncPin()), updateW1, RISING);
-  // attachPCINT(digitalPinToPCINT(w2.getEncPin()), updateW2, RISING);
-  // attachInterrupt(digitalPinToInterrupt(w3.getEncPin()), updateW3, RISING);
-  // attachInterrupt(digitalPinToInterrupt(w4.getEncPin()), updateW4, RISING);
 
-  Serial.begin(9600);
+  Serial.begin(115200);
   Serial.setTimeout(intervalMillis);
 }
 
-void userInput() {
-  int parsedInt = Serial.parseInt();
-  if (parsedInt != 0) {
-    parsedInt = constrain(parsedInt, -255, 256);
-    if (parsedInt == 256) { parsedInt = 0; }
-    vel = parsedInt;
-    Serial.print("Set velocity: ");
-    Serial.println(vel);
-  }
-}
-
-void runMotors() {
-  // u = w1.reachVelocity(vel);
-  // w1.setMotorValue(vel);
-  // w1.reachVelocity(vel);
-  // w2.reachVelocity(vel);
-  // w3.reachVelocity(vel);
-  // w4.reachVelocity(vel);
-  car.runForward(vel);
-}
-
-void sendData() {
-  Serial.print('$');
-
-  // Serial.print(vel);
-  // Serial.print(' ');
-  // Serial.print(u);
-  // Serial.print(' ');
-  // Serial.print(w1.getRPM(), 2);
-
-  Serial.print(car.getRPM1(), 2);
-  Serial.print(' ');
-  Serial.print(car.getRPM2(), 2);
-  Serial.print(' ');
-  Serial.print(car.getRPM3(), 2);
-  Serial.print(' ');
-  Serial.print(car.getRPM4(), 2);
-
-  // Serial.print(w1.getRPM(), 2);
-  // Serial.print(' ');
-  // Serial.print(w2.getRPM(), 2);
-  // Serial.print(' ');
-  // Serial.print(w3.getRPM(), 2);
-  // Serial.print(' ');
-  // Serial.print(w4.getRPM(), 2);
-
-  Serial.print(';');
-}
-
-void changeState() {
-  state = !state;
-  if(state) {
-    vel = 100;
-  } else {
-    vel = -100;
-  }
-}
-
 void loop() {
-  // userInput();
-
-  // auto t = micros();
-  runMotors();
-  // Serial.println("Time: " + (String)(micros() - t));
-
-  sendData();
-
-  now = millis();
-  if (now - prev > timer) {
-    prev = now;
-    changeState();
-  }
+  float* control_from_rpi = talker.rx();
+  car.setDesiredRpms(control_from_rpi);
+  car.updateFeedbackRpms();
+  talker.tx(car.getFeedbackRpms());
+  car.reachDesiredRpms();
 }