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:
-
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.
-
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:
