ESC control issues (Motor twitching periodically, always spinning)

I’m trying to make myself a drone.

Ok, so i have an arduino uno, an adafruit 9dof breakout IMU, a HC-06 BT module and 4 ESC’s/motors to control. I use an android app to send commands through BT. Currently i send the pitch and roll to the phone so it can do calculations and send them to the arduino in a “XXX,YYY,ZZZ,QQQ” format.

But i have a few issues:

  1. The motors always spin to some degree. I 1st used the Servo.write() function to drive the motors, but that didn’t go well, so i opened up librepilot and checked the timings so i could use Servo.writeMicroseconds() instead. a value of 1000 microseconds should make them stand still, but they just faintly spin instead.

  2. When spinning, the motors “twitch”, as in they stop/slow down and start spinning again. I’ve noticed that this happens during the communication phase of my program, but i don’t know exactly what causes it, or what to do to fix it.

Working on uploading a video showcasing the issues.

Here’s my code:

#include <Servo.h> 


#include <Adafruit_9DOF.h>

#include <Adafruit_LSM303.h>
#include <Adafruit_LSM303_U.h>

#include <Adafruit_L3GD20.h>
#include <Adafruit_L3GD20_U.h>

#include <SoftwareSerial.h>

#define INPUT_SIZE 16

#define NULL_SPEED 900
#define FULL_SPEED 1900

//currently unimplemented
int connectionCounter = 0;

String BTInput = "";

SoftwareSerial BT(12, 13); 

Servo motors[4];

int motorValues[] = {NULL_SPEED, NULL_SPEED, NULL_SPEED, NULL_SPEED};

//counter used to skip corrupted packets.
int values = 0;

double pitch;
double roll;

Adafruit_9DOF                dof   = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);

void initSensors(){
  {
  if(!accel.begin())
   {
     /* There was a problem detecting the LSM303 ... check your connections */
      Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
      while(1);
   }
   if(!mag.begin())
   {
      /* There was a problem detecting the LSM303 ... check your connections */
     Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
     while(1);
    }
  }
}

void setup() {
  BT.begin(9600);
  Serial.begin(9600);

  motors[0].attach(5);
  motors[1].attach(6);
  motors[2].attach(9);
  motors[3].attach(10);

  delay(2000);
  
  //bad attempts at arming sequence...

  for(int i=0; i<4; i++){
    motors[i].writeMicroseconds(NULL_SPEED);
  }
  
  delay(3000);
  for(int i=0; i<4; i++){
      motors[i].writeMicroseconds(1200);
  }
  delay(500);
  for(int i=0; i<4; i++){
    motors[i].writeMicroseconds(NULL_SPEED);
  }
  delay(1000);
  initSensors();
  accel.begin();
  
}

void loop() {

  
  sensors_event_t accel_event;
  
  //sending pitch and roll to phone
  accel.getEvent(&accel_event);
  if (dof.accelGetOrientation(&accel_event, &orientation))
  {
    char result[9] = "";
    char val1[5];
    char val2[5];
    char seperator[2] = {'|', 0};

    dtostrf(orientation.roll, 4, 0, val1);
    dtostrf(orientation.pitch, 4, 0, val2);

    strcat(result, val1);
    strcat(result, seperator);
    strcat(result, val2);
    Serial.print("result: ");
    Serial.println(result);
    
    BT.write(result);
  }

  //recieving control data from phone
  char input[INPUT_SIZE];
  if(BT.available()){
    BT.readStringUntil('|').toCharArray(input, INPUT_SIZE);
    
    //debug
    Serial.println(input);
    
    char* command = strtok(input, ",");

    while (command != 0){
      motorValues[values] = atoi(command);
      command = strtok(0, ",");
      values++;
    }

    //data check
    if(values==4){
      for(int i=0; i<4; i++){
        motorValues[i] *= 10; 
        Serial.println(motorValues[i]);
      }
      writeMotors();
    }
    values = 0;
    //currently unimplemented
    connectionCounter = 0;
  }
  else{
    //currently unimplemented
    connectionCounter++;
  }
  
  //Serial.println(v[0]);
  //delay(100);

  //debug
  delay(1000);
}

void writeMotors(){
  for(int i=0; i<4; i++){
    motors[i].writeMicroseconds(motorValues[i]);

    //debug
    Serial.println(motorValues[i]);
    //debug
    delay(20);
  }
}

the 2 videos:

What happens if you remove the communication piece? Write a sketch that talks to the 4 ESCs but nothing else. Run them up to speed for a few seconds, slow them down for a few seconds, stop for a few seconds.

Do you get your twitching and extraneous movements?

Another thought, you have several libraries in use. If any of them are using the same timer that the servo library uses, you might be in trouble.

Motors run just fine without the BT communication, even if i'm still using the adafruit libraries.

Did a bunch of searching after i tested everything and...

Well, you definately set me on the right track! SoftwareSerial and Servo apparently do not work well toghether because, from what understand, they both use interrupts. Sigh That would explain both the twitching and occasional gibberish in my BT coms...

I'm going to try getting around that, rewrite my android app to get calculation on the arduino again, and report once i'm done.

Well, completely removing SoftwareSerial has solved the issue! :D

Gonna be a pain to debug any future code now, but at least it's working. Thanks a lot.