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) {