init

2023-07-07 21:57:41 +02:00
commit dc5f14256e
12 changed files with 523 additions and 0 deletions
@@ -0,0 +1,76 @@
+#include <Servo.h>
+#include "DRONE.h"
+
+DRONE drone;
+
+unsigned long startTime, delayTime = 2000;  // in milliseconds
+int val = 0;
+bool gotSerialData = false;
+
+
+void setup() {
+  Serial.begin(115200);
+
+  startTime = micros();
+
+  // init radio
+  drone.radio.setup(9, 10); 
+
+  // init gyro
+  bool gyroStatus = drone.gyro.initGyro();
+  if (gyroStatus == false) {
+    Serial.println("Gyro init failed!");
+  } else {
+    drone.gyro.calculateError();
+  }
+
+  // set drone motorPins
+  drone.setPinESC(6, MotorPosition::FR);
+  drone.setPinESC(7, MotorPosition::FL);
+  drone.setPinESC(5, MotorPosition::BR);
+  drone.setPinESC(8, MotorPosition::BL);
+
+
+  Serial.println(String("Loop starts in 2 seconds ..."));
+  delay(2000);
+}
+
+void loop() {
+  String data = drone.radio.receive();
+
+  if (data.length() > 0) { 
+    Serial.println(data);
+  }
+
+  delay(50);
+
+  if (Serial.available() != 0) {
+    int tmp = Serial.parseInt();
+    if (tmp != 0) {
+      val = tmp;
+      gotSerialData = true;
+    }
+  }
+
+  if (val < MOTOR().minSpeed)
+    val = MOTOR().minSpeed;
+  else if (val > MOTOR().maxSpeed)
+    val = MOTOR().maxSpeed;
+
+  if ((millis() - startTime) >= delayTime && gotSerialData == true) {
+    drone.setSpeed(val, MotorPosition::FR);
+    drone.setSpeed(val, MotorPosition::FL);
+    drone.setSpeed(val, MotorPosition::BR);
+    drone.setSpeed(val, MotorPosition::BL);
+    // Serial.println(val);
+    startTime = micros();
+    gotSerialData = false;
+  }
+
+  // drone.update();
+
+  delay(10);
+  // delay(1000);
+}
+
+