Go Down

Topic: Problem with ServoLibrary (Read 474 times) previous topic - next topic

TheJokerzz

Hi,

I am currently trying to build a Quadrocopter based on the Arduino. The motors are controlled via Servo Library because the motorcontrollers listen to the same input signals as servos do.

Today I wanted to start my first Testflight and encountered a weird problem:

Whenever I have my Arduino connected to my Home PC (Windows 8) the sketch works fine and the motors are properly started etc. But when I try to power the Arduino via another Computer (Laptop, Ubuntu) or directly with an external powersupply the motors don't react at all. The rest of the sketch works fine (LED blinks when I want it to, Serial output works on Ubuntu as well). Its only the Servo Library that seems to stop working.

Any ideas what might cause this? Unfortunately I dont have another Windows Computer here to test if it's Windows that works or just my specific computer.

This is the sketch that is running on the Arduino:
Code: [Select]
#include <Arduino.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

#include "Ultrasonic.h"
#include <MegaServo.h>


//pins
#define LEDPIN 13
#define MOTOR0PIN 24
#define MOTOR1PIN 25
#define MOTOR2PIN 26
#define MOTOR3PIN 27
#define ULTRASONICPIN 28


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}


// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

//MPU data
MPU6050 mpu;
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
float yprDeg[3];

//Ultrasonic
Ultrasonic ultrasonic(ULTRASONICPIN);
long ultrasonicLastCheck = 0;
int height = 0;

MegaServo motor0, motor1, motor2, motor3;


//AI
bool motorsActive = false;
long motorLastWrite = 0;
int targetHeight = 20;
float targetYaw = 0, targetPitch = 0, targetRoll = 0;
int motorOffset = 956;
int motor0Offset, motor1Offset, motor2Offset, motor3Offset;
int motor0Speed, motor1Speed, motor2Speed, motor3Speed;




void setup() {
//init pins
pinMode(LEDPIN, OUTPUT);

//init communication
Serial.begin(115200);
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

//init sensors
mpu.initialize();
if (!mpu.testConnection())
{
while (true) {
delay(750);
digitalWrite(LEDPIN, HIGH);
delay(250);
digitalWrite(LEDPIN, LOW);
}
}
devStatus = mpu.dmpInitialize();

//init motors
motor0.attach(MOTOR0PIN);
motor1.attach(MOTOR1PIN);
motor2.attach(MOTOR2PIN);
motor3.attach(MOTOR3PIN);

motor0.writeMicroseconds(956);
motor1.writeMicroseconds(956);
motor2.writeMicroseconds(956);
motor3.writeMicroseconds(956);
digitalWrite(LEDPIN, HIGH);
delay(3000);
digitalWrite(LEDPIN, LOW);

if (devStatus == 0) {
mpu.setDMPEnabled(true);

attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
Serial.print(devStatus);

while (true) {
delay(750);
digitalWrite(LEDPIN, HIGH);
delay(250);
digitalWrite(LEDPIN, LOW);
}
}
}


void loop() {

        if(Serial.available() > 0) {
                motorsActive = !motorsActive;
                while(Serial.available() > 0) {
                        Serial.read();
                }
        }
 
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {

// check height every 200 ms
if (millis() - ultrasonicLastCheck > 200)
{
ultrasonicLastCheck = millis();
ultrasonic.DistanceMeasure();
height = ultrasonic.microsecondsToCentimeters();
/*
//output
Serial.print("height\t");
Serial.println(height);
*/
}

if (millis() - motorLastWrite > 50 && motorsActive)
{
motorLastWrite = millis();
// compute values for motors
yprDeg[0] = ypr[0] * 180 / M_PI;
yprDeg[1] = ypr[1] * 180 / M_PI;
yprDeg[2] = ypr[2] * 180 / M_PI;


if (height < targetHeight) {
motorOffset += 4;
}
else if (height > targetHeight) {
motorOffset -= 4;
}


if (yprDeg[0] > targetYaw) { // check yaw
motor0Offset += 4;
motor2Offset += 4;

motor1Offset -= 4;
motor3Offset -= 4;
}
else if (yprDeg[0] > targetYaw) {
motor0Offset -= 4;
motor2Offset -= 4;

motor1Offset += 4;
motor3Offset += 4;
}
                       
if (yprDeg[1] < targetPitch) { // check pitch
motor0Offset += 4;
motor3Offset += 4;

motor1Offset -= 4;
motor2Offset -= 4;
}
else if (yprDeg[1] > targetPitch) {
motor0Offset -= 4;
motor3Offset -= 4;

motor1Offset += 4;
motor2Offset += 4;
}

if (yprDeg[2] > targetRoll) { // check roll
motor0Offset += 4;
motor1Offset += 4;

motor2Offset -= 4;
motor3Offset -= 4;
}
else if (yprDeg[2] < targetRoll) {
motor0Offset -= 4;
motor1Offset -= 4;

motor2Offset += 4;
motor3Offset += 4;
}


motor0Offset = min(motor0Offset, 400);
motor0Offset = max(motor0Offset, -400);
motor1Offset = min(motor1Offset, 400);
motor1Offset = max(motor1Offset, -400);
motor2Offset = min(motor2Offset, 400);
motor2Offset = max(motor2Offset, -400);
motor3Offset = min(motor3Offset, 400);
motor3Offset = max(motor3Offset, -400);

motorOffset = min(motorOffset, 1600);
motorOffset = max(motorOffset, 956);

// write values to motors
motor0Speed = motorOffset + motor0Offset;
motor1Speed = motorOffset + motor1Offset;
motor2Speed = motorOffset + motor2Offset;
motor3Speed = motorOffset + motor3Offset;

motor0Speed = max(motor0Speed, 956);
motor1Speed = max(motor1Speed, 956);
motor2Speed = max(motor2Speed, 956);
motor3Speed = max(motor3Speed, 956);


                       
Serial.print(motor0Speed);
Serial.print("\t");
Serial.print(motor1Speed);
Serial.print("\t");
Serial.print(motor2Speed);
Serial.print("\t");
Serial.println(motor3Speed);
                       
                       
motor0.writeMicroseconds(motor0Speed);
motor1.writeMicroseconds(motor1Speed);
motor2.writeMicroseconds(motor2Speed);
motor3.writeMicroseconds(motor3Speed);
}

// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
}

//handle interrupt

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();


// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

                if(!motorsActive) {
        //output (not working in visualmicro)
        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180 / M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180 / M_PI);
        Serial.print("\t");
        Serial.println(ypr[2] * 180 / M_PI);
                }
}
}



Thanks for your help

Go Up