working version

DRONE.cpp

@@ -60,27 +60,52 @@ void DRONE::setSpeed(int speed, MotorPosition mp) {
 
 void DRONE::accelerate() {
   back_right.accelerate();
+  back_left.accelerate();
+  front_left.accelerate();
+  front_right.accelerate();
+}
+
+void DRONE::decelerate() {
+  back_right.decelerate();
+  back_left.decelerate();
+  front_left.decelerate();
+  front_right.decelerate();
+}
+
+void DRONE::stop() {
+  back_right.setSpeed(1000);
+  back_left.setSpeed(1000);
+  front_left.setSpeed(1000);
+  front_right.setSpeed(1000);
+}
+
+String DRONE::getSpeedFromMotor(MotorPosition mp) {
+  switch (mp) {
+    case BL:
+      return "BL:"+String(back_left.getSpeed());
+      break;
+    case BR:
+      return "BR:"+String(back_right.getSpeed());
+      break;
+    case FL:
+      return "FL:"+String(front_left.getSpeed());
+      break;
+    case FR:
+      return "FR:"+String(front_right.getSpeed());
+      break;
+  }
+  return "";
 }
 
 
 void DRONE::update() {
   float* mpuValues = gyro.getAverages();
   
+  // for debugging
   // 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) {

DRONE.h

@@ -24,6 +24,9 @@ class DRONE {
     void setPinESC(int pin, MotorPosition mp);
     void setSpeed(int speed, MotorPosition mp);
     void accelerate();
+    void decelerate();
+    void stop();
+    String getSpeedFromMotor(MotorPosition mp);
     bool initReceiver(int cepin, int cspin);
     void update(); 
 };

DroneReceiver.ino

@@ -7,6 +7,10 @@ unsigned long startTime, delayTime = 2000;  // in milliseconds
 int val = 0;
 bool gotSerialData = false;
 
+int currentMsgIndex = 0;
+
+unsigned long lastReceivedMessage = 0;  // Variable to store the previous time
+unsigned long maxMessageDelay = 3000;
 
 void setup() {
   Serial.begin(115200);
@@ -31,18 +35,71 @@ void setup() {
   drone.setPinESC(8, MotorPosition::BL);
 
 
-  Serial.println(String("Loop starts in 2 seconds ..."));
-  delay(2000);
+  lastReceivedMessage = millis();
+  Serial.println(String("Loop starts now"));
 }
 
 void loop() {
-  String data = drone.radio.receive();
-
-  if (data.length() > 0) { 
-    Serial.println(data);
+  String textToSend = "Alive";  
+  switch(currentMsgIndex) {
+    case 1:
+      textToSend = drone.getSpeedFromMotor(BL);
+      break;
+    case 2:
+      textToSend = drone.getSpeedFromMotor(BR);
+      break;
+    case 3:
+      textToSend = drone.getSpeedFromMotor(FL);
+      break;
+    case 4:
+      textToSend = drone.getSpeedFromMotor(FR);
+      break;
   }
 
-  delay(50);
+  if (currentMsgIndex > 4) {
+    currentMsgIndex = 1;
+  } else {
+    currentMsgIndex ++;
+  }
+
+  // Serial.println(textToSend);
+  String recText = drone.radio.receiveAck(textToSend);
+
+  if (recText.length() > 0) { 
+    lastReceivedMessage = millis();
+    // Serial.println(recText);
+
+    const char delimiter[] = "|";
+    char* token = strtok(recText.c_str(), delimiter);
+    while (token != NULL) {
+      // read the token here
+      String command = String(token);
+      if (command == "Stop") {
+          drone.stop();
+      } else if (command == "Up") {
+          drone.accelerate();
+      } else if (command == "Down") {
+          drone.decelerate();
+      }
+      token=strtok(NULL, delimiter);
+    }
+  } else {
+    // if no message is received then slow down the motors
+    if (millis() - lastReceivedMessage >= maxMessageDelay) {
+      Serial.println("Lost Connection");
+      drone.stop();
+    }
+  }
+
+
+
+  drone.update();
+
+  delay(10);
+  // delay(1000);
+}
+
+void manualMotorSpeed() {
   
   if (Serial.available() != 0) {
     int tmp = Serial.parseInt();
@@ -66,11 +123,5 @@ void loop() {
     startTime = micros();
     gotSerialData = false;
   }
-
-  // drone.update();
-
-  delay(10);
-  // delay(1000);
 }
 
-

RadioController.cpp

@@ -8,12 +8,45 @@ void RadioController::setup(int cepin, int cspin) {
     Serial.println("Error initializing radio");
   } else {
     Serial.println("Succesfully initializing radio");
-    radio.openWritingPipe(writePipe); 
-    radio.openReadingPipe(1, readPipe);
-    radio.setPALevel(RF24_PA_MAX);
-    radio.disableDynamicPayloads();
-    radio.setAutoAck(false); 
-    radio.setRetries(5, 15);
+    // radio.openWritingPipe(writePipe); 
+    // radio.openReadingPipe(1, readPipe);
+    // radio.setPALevel(RF24_PA_MAX);
+    // radio.disableDynamicPayloads();
+    // radio.setAutoAck(false); 
+    // radio.setRetries(5, 15);
+
+    
+    // radio.setDataRate( RF24_250KBPS );
+    // radio.openReadingPipe(1, readPipe);
+
+    // radio.enableAckPayload();
+    
+    // radio.startListening();
+
+    uint8_t address[][6] = { "1Node", "2Node" };
+    
+    // Set the PA Level low to try preventing power supply related problems
+    // because these examples are likely run with nodes in close proximity to
+    // each other.
+    radio.setPALevel(RF24_PA_LOW);  // RF24_PA_MAX is default.
+
+    // to use ACK payloads, we need to enable dynamic payload lengths (for all nodes)
+    radio.enableDynamicPayloads();  // ACK payloads are dynamically sized
+
+    // Acknowledgement packets have no payloads by default. We need to enable
+    // this feature for all nodes (TX & RX) to use ACK payloads.
+    radio.enableAckPayload();
+
+    // set the TX address of the RX node into the TX pipe
+    radio.openWritingPipe(address[1]);  // always uses pipe 0
+
+    // set the RX address of the TX node into a RX pipe
+    radio.openReadingPipe(1, address[0]);  // using pipe 1
+
+    // load the payload for the first received transmission on pipe 0
+    // radio.writeAckPayload(1, &payload, sizeof(payload));
+
+    radio.startListening();  // put radio in RX mode
   }
 }
 
@@ -30,7 +63,6 @@ void RadioController::send(String msg) {
 }
 
 String RadioController::receive() {
-  radio.startListening();
   String msg = "";
 
   if (radio.available()) {
@@ -41,3 +73,19 @@ String RadioController::receive() {
 
   return msg;
 }
+String RadioController::receiveAck(String textToSend) {
+  String msg = "";
+
+  if (radio.available()) {
+    char receivedText[32] = ""; 
+    uint8_t bytes = radio.getDynamicPayloadSize();  // get the size of the payload
+    radio.read(&receivedText, bytes);  // read only the first 'bytes' from the payload
+    msg = String(receivedText); 
+
+    char ack[8]; 
+    textToSend.substring(0, 8).toCharArray(ack, sizeof(ack));
+    radio.writeAckPayload(1, &ack, sizeof(ack));
+  }
+
+  return msg;
+}

RadioController.h

@@ -16,6 +16,7 @@ class RadioController {
     void setup(int cepin, int cspin);
     void send(String msg);
     String receive(); 
+    String receiveAck(String textToSend);
 };
 
 #endif