Hello everyone, i have found this sketch on the internet:
int count=0;
void loop()
{
count++;
sampleTimePID = (micros()-timer)/1000000.0/CC_FACTOR; // in Seconds!
timer = micros();
// Update raw Gyro
updateRawGyroData(&gyroRoll,&gyroPitch);
// Update ACC data approximately at 50Hz to save calculation time.
if(count == 20)
{
mpu.getAcceleration(&x_val,&y_val,&z_val);
sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
timerACC=timer;
//{Serial.print(sampleTimeACC,5);Serial.print(" ");Serial.println(sampleTimePID,5);}
}
if(count == 21) rollAngleACC =atan2(-y_val,-z_val)*57.2957795;
if(count == 22)
{
pitchAngleACC =-atan2(-x_val,-z_val)*57.2957795;
count=0;
}
//Serial.println( (micros()-timer)/CC_FACTOR);
gyroRoll = gyroRoll + config.accelWeight * (rollAngleACC - rollSetpoint)/sampleTimeACC;
gyroPitch = gyroPitch + config.accelWeight * (pitchAngleACC - pitchSetpoint)/sampleTimeACC;
pitchPID = ComputePID(sampleTimePID,gyroPitch,0.0, &pitchErrorSum, &pitchErrorOld,config.gyroPitchKp,config.gyroPitchKi,config.gyroPitchKd,maxDegPerSecondPitch);
rollPID = ComputePID(sampleTimePID,gyroRoll,0.0, &rollErrorSum, &rollErrorOld,config.gyroRollKp,config.gyroRollKi,config.gyroRollKd,maxDegPerSecondRoll);
pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000);
pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000);
rollDirection = sgn(rollDevider) * config.dirMotorRoll;
// Serial.println(freeRam ());
sCmd.readSerial();
}
The Sketch main function is to read MPU6050 data. But from that code, I can not understand about "count'". On the Loop section, there is some "count" code, like:
count++;
if(count == 21) rollAngleACC =atan2(-y_val,-z_val)*57.2957795;
if(count == 22)
{
pitchAngleACC =-atan2(-x_val,-z_val)*57.2957795;
count=0;
}
count=0;
My question is: What the function of "count" code? And what is the different is we not use "count" on the loop section?
Thank You..
Oh, i found similiar code on the ISR function too. They are "freqCounter", "deviderCountPitch", and "deviderCountRoll" :
ISR( TIMER1_OVF_vect )
{
freqCounter++;
if(freqCounter==(CC_FACTOR/MOTORUPDATE_FREQ))
{
// Move pitch and roll Motor
deviderCountPitch++;
if(deviderCountPitch >= abs(pitchDevider))
{
fastMoveMotor(MOTOR_PITCH, pitchDirection);
deviderCountPitch=0;
}
deviderCountRoll++;
if(deviderCountRoll >= abs(rollDevider))
{
fastMoveMotor(MOTOR_ROLL, rollDirection);
deviderCountRoll=0;
}
freqCounter=0;
}
}