I am currently programming a FlightController for a Quadrocopter. For some reason if I connect the battery to the drone the calibraton of the ESC works perfectly. If I try to turn on the motors nothing happens (not even the TX LED on the Arduino blinks). If I connect the Board to a PC and open the Serial Monitor I see that after the initialisiation of the receiver there is no Serial ouptput. If I now press restart everythig works just fine. If I disconnect the board from the PC everything still works (if I don't disconnect the battery). If I press restart now it sometimes works and sometimes it doesn't (still didn't disconnect the battery). If I disconnect the battery and reconnect it it doesn't work until I connect it to a PC again. Also I somehow have to put those empty Serial.println in order to be any Serial outputafter the initialisation of the Receiver. I have no clue what is going on here. Did I mess up using an array Maby you can find an error in my Code.
FlightController1.1.ino:
#include"Returnvalues.h"
#include"Receiver.h"
#include"ESC.h"
#include<SPI.h>
//---- global variable declarations ----
ESC* esc;
Receiver* receiver;
int motorPowers[4];
//---- function declarations ----
void shutdown();
void resetMotorPowers();
void up(int * pow, int sens, int acc);
void down(int * pow, int sens, int acc);
void convertInput(int *leftStickInput, int *rightStickInput);
void printMotorPowers();
//---- funktion definitions ----
void shutdown() {
delete receiver;
delete esc;
while (1);
}
void resetMotorPowers() {
for (int i = 0; i < 4; i++) {
motorPowers[i] = 0;
}
}
void up(int * pow, int sens, int acc) {
if (motorPowers[0] < 180 && motorPowers[1] < 180 && motorPowers[2] < 180 && *pow > 0) {
for (int i = 0; i < 4; i++)
motorPowers[i]++;
delay(acc + sens - map(*pow, 1, 124, 1 , sens)); //176
}
}
void down(int * pow, int sens, int acc) {
if (motorPowers[0] > 0 && motorPowers[1] > 0 && motorPowers[2] > 0 && *pow < 0) {
for (int i = 0; i < 4; i++)
motorPowers[i]--;
delay(acc + sens + map(*pow, -1, -131, -1 , -sens)); //176
}
}
void convertInput(int *leftStickInput, int *rightStickInput, int sens, int acc) {
up(&rightStickInput[1], sens, acc);
down(&rightStickInput[1], sens, acc);
}
void printMotorPowers() {
Serial.println("M1: " + String(motorPowers[0])
+ "M2: " + String(motorPowers[1])
+ "M3: " + String(motorPowers[2])
+ "M4: " + String(motorPowers[3]));
}
//---- setup + loop ----
void setup() {
Serial.begin(9600);
esc = new ESC(10, 9, 6, 5);
receiver = new Receiver;
resetMotorPowers();
}
void loop() {
int status = ReturnValues::Return_OK;
status = receiver->routine(RIGHT_STICK, 125, 127, false);
if (status == ReturnValues::Return_PROBLEM) {
//esc->connectionLost();
} else if (status == ReturnValues::Return_FATAL) {
shutdown();
}
convertInput(receiver->getControllerOutput(ControllerSticks::LEFT_STICK), receiver->getControllerOutput(ControllerSticks::RIGHT_STICK), 1000, 70);
Serial.print(""); //why has this to be be here???????
printMotorPowers();
Serial.print("");
esc->update(motorPowers);
}
ESC.cpp:
#include"Returnvalues.h"
#include"ESC.h"
#include<Servo.h>
#include<SPI.h>
ESC::ESC(int pinMotor1, int pinMotor2, int pinMotor3, int pinMotor4) {
pins[0] = pinMotor1;
pins[1] = pinMotor2;
pins[2] = pinMotor3;
pins[3] = pinMotor4;
Serial.println("start ESC setup");
delay(2000);
attachMotors();
calibrate();
Serial.println("Motors armed -- ESC Setup completed");
}
void ESC::attachMotors() {
for (int i = 0; i < 4; i++) {
motors[i].attach(pins[i], 1000, 2000);
Serial.println("Motor" + String(i) + " attached");
}
}
void ESC::calibrate() {
Serial.println("start calibration");
for (int i = 0; i <= 180; i++) {
for (int k = 0; k < 4; k++) {
motors[k].write(i);
}
delay(10);
}
Serial.println("max value reached -- phase 1 completed");
delay(2000);
for (int i = 180; i >= 0; i--) {
for (int k = 0; k < 4; k++) {
motors[k].write(i);
}
delay(10);
}
Serial.println("min value reached -- phase 2 completed");
delay(2000);
Serial.println("calibration completed");
}
void ESC::shutdownMotors() {
for (int i = 0; i < 4; i++)
motors[i].write(0);
}
int ESC::manuelWrite(int motor, int value) {
if (motor < 0 || motor > 3) {
Serial.println("ESC.ccp::manuelWrite::not supported Input value. ABBORT");
return ReturnValues::Return_WRONG_INPUT;
}
motors[motor].write(value);
return ReturnValues::Return_OK;
}
void ESC::update(int *motorPowers) {
for (int i = 0; i < 4; i++) {
motors[i].write(motorPowers[i]);
}
}
ESC::~ESC() {
shutdownMotors();
for (int i = 0; i < 4; i++) {
motors[i].detach();
}
}
ESC.h:
#ifndef ESC_H
#define ESC_H
#include<Servo.h>
class ESC
{
private:
int pins[4];
Servo motors[4];
void attachMotors();
void calibrate();
public:
ESC(int pinMotor1, int pinMotor2, int pinMotor3, int pinMotor4);
void shutdownMotors();
int manuelWrite(int motor, int value);
void update(int *motorPowers);
~ESC();
};
#endif
Receiver.cpp:
#include"Receiver.h"
#include"Returnvalues.h"
#include<RH_ASK.h>
#include<SPI.h>
Receiver::Receiver() {
if (!RX.init()) {
Serial.println("RF-Modul could not be initialised");
} else {
Serial.println("RF_Modul succesfully initialised");
}
}
int Receiver::recvControllerOutput(int stick, int xCalibration, int yCalibration, bool print) {
uint8_t buf[3];
uint8_t bufLen = sizeof(buf);
int status = ReturnValues::Return_PROBLEM;
if (stick == LEFT_STICK) {
if (RX.recv(buf, &bufLen)) {
status = Return_OK;
for (int i = 0; i < 3; i++) {
leftStick[i] = buf[i];
}
leftStick[0] = (leftStick[0] - xCalibration) * (-1);
leftStick[1] = leftStick[1] - yCalibration;
if (print) {
printControllerOutput(LEFT_STICK);
}
}
} else if (stick == RIGHT_STICK) {
if (RX.recv(buf, &bufLen)) {
status = Return_OK;
for (int i = 0; i < 3; i++) {
rightStick[i] = buf[i];
}
rightStick[0] = (rightStick[0] - xCalibration) * (-1);
rightStick[1] = rightStick[1] - yCalibration;
if (print) {
printControllerOutput(RIGHT_STICK);
}
}
}
return status;
}
void Receiver::printControllerOutput(int stick) {
if (stick == LEFT_STICK) {
Serial.print("X: ");
Serial.print(leftStick[0], DEC);
Serial.print(" Y: ");
Serial.print(leftStick[1], DEC);
Serial.print(" Z: ");
Serial.println(leftStick[2], DEC);
} else if (stick == RIGHT_STICK) {
Serial.print("X: ");
Serial.print(rightStick[0], DEC);
Serial.print(" Y: ");
Serial.print(rightStick[1], DEC);
Serial.print(" Z: ");
Serial.println(rightStick[2], DEC);
}
}
bool Receiver::isEmergency() {
if (rightStick[2] == 1) {
return true;
}
return false;
}
int Receiver::routine(int stick, int xCalibration, int yCalibration, bool print) {
int status = ReturnValues::Return_OK;
if (recvControllerOutput(stick, xCalibration, yCalibration, print) == ReturnValues::Return_PROBLEM)
status = ReturnValues::Return_PROBLEM;
if (isEmergency())
status = ReturnValues::Return_FATAL;
return status;
}
void Receiver::resetOutputStream() {
for (int i = 0; i < 3; i++) {
leftStick[i] = 0;
rightStick[i] = 0;
}
}
int* Receiver::getControllerOutput(int stick) {
if (stick == LEFT_STICK) {
return leftStick;
}
return rightStick;
}
Receiver::~Receiver() {
resetOutputStream();
};
Receiver.h:
#ifndef RECEIVER_H
#define RECEIVER_H
#include<RH_ASK.h>
class Receiver
{
private:
int leftStick[3];
int rightStick[3];
RH_ASK RX;
public:
Receiver();
int recvControllerOutput(int stick, int xCalibration, int yCalibration, bool print);
//void printControllerOutput();
void printControllerOutput(int stick);
int routine(int stick, int xCalibration, int yCalibration, bool print);
void resetOutputStream();
int *getControllerOutput(int stick);
bool isEmergency();
~Receiver();
};
enum ControllerSticks {
LEFT_STICK = 0,
RIGHT_STICK = 1,
};
#endif
Returnvalues.h:
#ifndef RETURNVALUES_H
#define RETURNVALUES_H
enum ReturnValues {
Return_OK = 0,
Return_FATAL = 1,
Return_PROBLEM = 2,
Return_WRONG_INPUT = 3,
};
#endif
It is not a hardware problem because this skript works perfectly fine:
#include<Servo.h>
#include <RH_ASK.h>
#include <SPI.h>
#define value 80
bool emergencyShutdown = false;
bool momentaryRecive = false;
RH_ASK RX;
int controllerL[3];
Servo m1;
Servo m2;
Servo m3;
Servo m4;
int mPow [4] = {0,0,0,0};
int mPowDelay = 10;
void setup() {
Serial.begin(9600);
escSetup();
rxSetup();
}
void loop() {
recvControllerInput(controllerL, 125, 127, true);
if (controllerL[2] == 1) {
shutdownMotors();
while(1);
}
up(&controllerL[1], 1000, 70);
down(&controllerL[1], 1000, 70);
updateESC();
}
void rxSetup() {
if (!RX.init()) {
Serial.println("RF-Modul konnte nicht initialisiert werden");
} else {
Serial.println("RF-Modul erfolgreich initialisiert");
}
}
void escSetup() {
Serial.println("start ESC setup");
delay(2000);
m1.attach(10,1000,2000);
Serial.println("attached Motor 1");
m2.attach(9,1000,2000);
Serial.println("attached Motor 2");
m3.attach(6,1000,2000);
Serial.println("attached Motor 3");
m4.attach(5,1000,2000);
Serial.println("attached Motor 4");
Serial.println("start calibration");
for(int i = 0; i <= 180 ; i++){
m1.write(i);
m2.write(i);
m3.write(i);
m4.write(i);
delay(10);
}
Serial.println("max value reached -- phase 1 completed");
delay(2000);
for(int i = 180; i >= 0; i--){
m1.write(i);
m2.write(i);
m3.write(i);
m4.write(i);
delay(10);
}
Serial.println("min value reached -- phase 2 completed");
delay(2000);
Serial.println("Motors armed -- ESC Setup completed");
Serial.println("start m1");
m1.write(value);
Serial.println("start m2");
m2.write(value);
Serial.println("start m3");
m3.write(value);
Serial.println("start m4");
m4.write(value);
}
void recvControllerInput(int * output, int xCalibration, int yCalibration, bool print) {
uint8_t buf[3];
uint8_t bufLen = sizeof(buf);
if (RX.recv(buf, &bufLen)) {
momentaryRecive = true;
for (int i = 0; i < 3; i++)
*(output + i) = buf[i];
*output = (*output - xCalibration) * (-1);
*(output + 1) = *(output + 1) - yCalibration;
if (print == true) {
printControllerInput(output);
printMotorPower();
}
} else {
momentaryRecive = false;
}
}
void printControllerInput(int * arr) {
Serial.print("X: ");
Serial.print(*arr, DEC);
Serial.print(" Y: ");
Serial.print(*(arr + 1), DEC);
Serial.print(" Z: ");
Serial.println(*(arr + 2), DEC);
}
void up(int * pow, int sens, int acc) {
if (mPow[0] < 180 && mPow[1] < 180 && mPow[2] < 180 && *pow > 0) {
for (int i = 0; i < 4; i++)
mPow[i]++;
delay(acc + sens - map(*pow, 1, 124, 1 , sens)); //176
}
}
void down(int * pow, int sens, int acc) {
if (mPow[0] > 0 && mPow[1] > 0 && mPow[2] > 0 && *pow < 0) {
for (int i = 0; i < 4; i++)
mPow[i]--;
delay(acc + sens + map(*pow, -1, -131, -1 , -sens)); //176
}
}
void printMotorPower() {
Serial.print("M1: ");
Serial.print(mPow[0], DEC);
Serial.print(" M2: ");
Serial.print(mPow[1], DEC);
Serial.print(" M3: ");
Serial.print(mPow[2], DEC);
Serial.print(" M4: ");
Serial.println(mPow[3], DEC);
}
void updateESC() {
m1.write(mPow[0]);
m2.write(mPow[1]);
m3.write(mPow[2]);
m4.write(mPow[3]);
}
void shutdownMotors() {
m1.write(0);
m2.write(0);
m3.write(0);
m4.write(0);
}