Inverted Pendulum Robot

// Inverted pendulum (self balancing robot)
int i = 0; // Iteration counter
byte countS = 0; // ??
double zeroOmegaI = 0; // ??
double recOmegaI[10]; // ??
double omegaI = 0; // Rotational velocity
double thetaI = 0; // Rotational angle
double sumPower = 0; // ??
double sumSumP = 0; // ??
const double kAngle = 2.5; // (54/200)
const double kOmega = 6; // (170/78)
const double kSpeed = .6; // (60/1000)
const double kDistance = .6; // (60/1000)
double powerScale; // ??
double power; // The power to run the motors at
double vE5 = 0; // ??
double xE5 = 0; // ??

void setup () {
    Serial.begin(115200); 
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    pinMode(6, OUTPUT);
    pinMode(7, OUTPUT);
    pinMode(8, OUTPUT);
    pinMode(9, OUTPUT);
    
    for ( i = 0 ; i < 10 ; i++ ) { 
        recOmegaI[i] = 0; 
    } 

    delay(300);
    training();
}

void loop () {
    chkAndCtl();
    log();
    if ( power > 0 ) {
        analogWrite( 6, power ); 
        digitalWrite( 4, HIGH ); 
        digitalWrite( 5, LOW );
        
        analogWrite( 9, power ); 
        digitalWrite( 7, HIGH ); 
        digitalWrite( 8, LOW );
    } else {
        analogWrite( 6, -power );
        digitalWrite( 4, LOW ); 
        digitalWrite( 5, HIGH ); 
        
        analogWrite( 9, -power ); 
        digitalWrite( 7, LOW ); 
        digitalWrite( 8, HIGH );
    }
    
    delayMicroseconds( 3600 ); //3600
}

void log() {
    Serial.print( power );
    Serial.println();
}

void training() { 
    delay (1000);

    for ( i = 0 ; i < 500 ; i++ ) {
        zeroOmegaI = zeroOmegaI + analogRead( A5 ); 
    }
    
    zeroOmegaI = zeroOmegaI / i;
}

void chkAndCtl() {
    omegaI = 0;

    for ( i = 0 ; i < 10 ; i++ ) {
        omegaI = omegaI + analogRead( A5 ) - zeroOmegaI;
        delayMicroseconds(10); 
    }
  
    omegaI = omegaI / 10;
    
    if (abs( omegaI ) < 3 ) { 
        omegaI = 0; 
    }
    
    thetaI = thetaI + omegaI;
    countS = 0;
    
    for ( i = 0 ; i < 10 ; i++ ) {
        if  (abs( recOmegaI[ i ] ) < 8 ) { 
            countS++;
        }
    }

    if ( countS > 9 ) {
        thetaI = 0;
        vE5 = 0;
        xE5 = 0; 
        sumPower = 0;
        sumSumP = 0; 
    }
    
    for ( i = 9 ; i > 0 ; i-- ) { 
        recOmegaI[ i ] = recOmegaI[ i - 1 ]; 
    }

    powerScale = ( kAngle * thetaI ) + ( kOmega * omegaI ) + ( kSpeed * vE5 ) + ( kDistance * xE5 );
    power = max( min( powerScale, 255.0 ) , -255.0 ); 
    sumPower = sumPower + power; // add int here for bias
    sumSumP = sumSumP + sumPower;
    vE5 = sumPower;
    xE5 = sumSumP / 100.0;
}
// Based on code by ArduinoDeXXX