init

2023-07-07 21:57:41 +02:00
commit dc5f14256e
12 changed files with 523 additions and 0 deletions
@@ -0,0 +1,149 @@
+// DRONE.cpp
+#include "Arduino.h"
+#include "HardwareSerial.h"
+#include "DRONE.h"
+
+MotorGyro motorGyro[] = {
+  {pitchStatus: MINUS, rollStatus: IDLE, accelerate: {FL, FR}, decelerate: {BL, BR}},
+  {pitchStatus: PLUS, rollStatus: IDLE, accelerate: {BL, BR}, decelerate: {FL, FR}},
+  
+  {pitchStatus: MINUS, rollStatus: MINUS, accelerate: {FR}, decelerate: {BL}},
+  {pitchStatus: PLUS, rollStatus: PLUS, accelerate: {BL}, decelerate: {FR}},
+
+  {pitchStatus: MINUS, rollStatus: PLUS, accelerate: {FL}, decelerate: {BR}},
+  {pitchStatus: PLUS, rollStatus: MINUS, accelerate: {BR}, decelerate: {FL}},
+
+  {pitchStatus: IDLE, rollStatus: MINUS, accelerate: {BR, FR}, decelerate: {BL, FL}},
+  {pitchStatus: IDLE, rollStatus: PLUS, accelerate: {BL, FL}, decelerate: {BR, FR}},
+};
+
+DRONE::DRONE() {
+}
+
+void DRONE::setPinESC(int pin, MotorPosition mp) {
+  switch (mp) {
+    case BR:
+      back_right.setMotorPosition(mp);
+      back_right.setPinESC(pin); 
+      break;
+    case BL:
+      back_left.setMotorPosition(mp);
+      back_left.setPinESC(pin);
+      break;
+    case FR:
+      front_right.setMotorPosition(mp);
+      front_right.setPinESC(pin);
+      break;
+    case FL:
+      front_left.setMotorPosition(mp);
+      front_left.setPinESC(pin);
+      break;
+  }
+}
+
+void DRONE::setSpeed(int speed, MotorPosition mp) {
+  switch (mp) {
+    case MotorPosition::BR:
+      back_right.setSpeed(speed);
+      break;
+    case MotorPosition::BL:
+      back_left.setSpeed(speed);
+      break;
+    case MotorPosition::FR:
+      front_right.setSpeed(speed);
+      break;
+    case MotorPosition::FL:
+      front_left.setSpeed(speed);
+      break;
+  }
+}
+
+void DRONE::accelerate() {
+  back_right.accelerate();
+}
+
+
+void DRONE::update() {
+  float* mpuValues = gyro.getAverages();
+
+  // Serial.println("->Pitch:" + String(mpuValues[0]) + "|Roll:" + String(mpuValues[1])); 
+
+  // todo send one line with all important values back to the controller via radio
+
+  String msg = "PT:" + String(mpuValues[0]) + "|RL:"+ String(mpuValues[1]);
+
+  updateMotors(mpuValues);
+
+  // msg += "|MP_FL:"+String(front_left.getSpeed());
+  // msg += "|MP_FR:"+String(front_right.getSpeed());
+  // msg += "|MP_BL:"+String(back_left.getSpeed());
+  // msg += "|MP_BR:"+String(back_right.getSpeed());
+  // Serial.println(msg);
+
+  radio.send(msg);
+}
+
+void DRONE::updateMotors(float* mpuValues) {
+  float pitch = mpuValues[0];
+  float roll = mpuValues[1];
+
+  float threshold = 0.3;
+
+  PitchRollStates pitchStatus = IDLE;
+  if (pitch < -threshold) {
+    pitchStatus = MINUS;
+  } else if (pitch > threshold) {
+    pitchStatus = PLUS;
+  }
+
+  PitchRollStates rollStatus = IDLE;
+  if (roll < -threshold) {
+    rollStatus = MINUS;
+  } else if (roll > threshold) {
+    rollStatus = PLUS;
+  }
+
+  // Serial.println("PS: " + String(pitchStatus) + " | RS: " + String(rollStatus)); 
+
+  int numCond = sizeof(motorGyro) / sizeof(motorGyro[0]);
+  for (int i = 0; i < numCond; i++) {
+    MotorGyro currentMotorGyro = motorGyro[i];
+    
+    if (currentMotorGyro.pitchStatus == pitchStatus && currentMotorGyro.rollStatus == rollStatus) {
+      for(int p = 0; p < 4; p++) {
+        switch(currentMotorGyro.accelerate[p]) {
+          case BL:
+            back_left.accelerate(); 
+            break;
+          case BR:
+            back_right.accelerate(); 
+            break;
+          case FL:
+            front_left.accelerate();
+            break;
+          case FR:
+            front_right.accelerate();
+            break;
+        }
+      }
+      for(int r = 0; r < 4; r++) {
+        switch(currentMotorGyro.decelerate[r]) {
+          case BL:
+            back_left.decelerate(); 
+            break;
+          case BR:
+            back_right.decelerate(); 
+            break;
+          case FL:
+            front_left.decelerate();
+            break;
+          case FR:
+            front_right.decelerate();
+            break;
+        }
+      }
+      break;
+    }
+  }
+  delete[] mpuValues;
+}