I'm in the middle of writing my own quadcopter code, I finally finished the quadcopter stability code.
BUT I noticed that cycle loop is very slow(10hz) compared to the maximum speed(200hz)
it's mean that im sending 10 commands to the motors per secound...
Does anyone know if this is enough?(My code is pretty short and neat)
I have not tried it on the device yet.
im using APM 2.6.
void loop()
{
time2 = millis();
dtime2 = time2 -ltime2;
ltime2 = time2;
desiredAngle0 = 0;
desiredAngle1 = 0;
angleSmoothDevider = 10;
if (Serial.available() > 0) {
generalPower = Serial.read();
Serial.println(generalPower);
}
if(gyro(0) != desiredAngle0){
desiredDPS0=pow(gyro(0)-desiredAngle0,2);
if(gyro(0) > desiredAngle0){
desiredDPS0*=-1;
}
}
if(gyro(1) != desiredAngle1){
desiredDPS1=pow(gyro(1)-desiredAngle1,2);
if(gyro(1) > desiredAngle1){
desiredDPS1*=-1;
}
}
if(gyro(3) != desiredDPS0){
if(gyro(3) < desiredDPS0){
angleBalanceHelper = (desiredDPS0-gyro(3))/angleSmoothDevider;
motor2Val+=angleBalanceHelper;
motor4Val+=angleBalanceHelper;
motor1Val-=angleBalanceHelper;
motor3Val-=angleBalanceHelper;
}
else{
angleBalanceHelper = (gyro(3)-desiredDPS0)/angleSmoothDevider;
motor2Val-=angleBalanceHelper;
motor4Val-=angleBalanceHelper;
motor1Val+=angleBalanceHelper;
motor3Val+=angleBalanceHelper;
}
}
if(gyro(4) != desiredDPS1){
if(gyro(4) < desiredDPS1){
angleBalanceHelper = (desiredDPS1-gyro(4))/angleSmoothDevider;
motor3Val+=angleBalanceHelper;
motor4Val+=angleBalanceHelper;
motor1Val-=angleBalanceHelper;
motor2Val-=angleBalanceHelper;
}
else{
angleBalanceHelper = (gyro(4)-desiredDPS1)/angleSmoothDevider;
motor3Val-=angleBalanceHelper;
motor4Val-=angleBalanceHelper;
motor1Val+=angleBalanceHelper;
motor2Val+=angleBalanceHelper;
}
}
int motorsGeneralPower = (motor1Val+motor2Val+motor3Val+motor4Val)/4;
if(motorsGeneralPower != generalPower){
motor1Val-=motorsGeneralPower-generalPower;
motor2Val-=motorsGeneralPower-generalPower;
motor3Val-=motorsGeneralPower-generalPower;
motor4Val-=motorsGeneralPower-generalPower;
}
if(255-generalPower > generalPower-1){
minMotorVal=1;
maxMotorVal=2*generalPower-1;
}
if(255-generalPower < generalPower-1){
minMotorVal=generalPower-255-generalPower;
maxMotorVal=255;
}
if(motor1Val > maxMotorVal) motor1Val=maxMotorVal;
if(motor2Val > maxMotorVal) motor2Val=maxMotorVal;
if(motor3Val > maxMotorVal) motor3Val=maxMotorVal;
if(motor4Val > maxMotorVal) motor4Val=maxMotorVal;
if(motor1Val < minMotorVal) motor1Val=minMotorVal;
if(motor2Val < minMotorVal) motor2Val=minMotorVal;
if(motor3Val < minMotorVal) motor3Val=minMotorVal;
if(motor4Val < minMotorVal) motor4Val=minMotorVal;
Serial.println();
Serial.print(motor1Val);
Serial.print(", ");
Serial.print(motor2Val);
Serial.print(", ");
Serial.print(motor3Val);
Serial.print(", ");
Serial.print(motor4Val);
Serial.print(", Cycles Per Second: ");
Serial.print(cyclesPerSecond);
cyclesPerSecond = 1/(dtime2*0.001);
dtime2=0;
/*
Serial.print("Position: ");
Serial.print("lat: ");
Serial.print(GPS(0));
Serial.print(" ");
Serial.print("lon: ");
Serial.println(GPS(1)); */
/*
Serial.println();
Serial.print(Altitude());
Serial.print(",");*/
}