File content as of revision 112:717acc7c99f9:

#ifndef SWING_H
#define SWING_H

/***Swing.***/
PID contSwing(20.917 ,2.0 ,0.0 ,RATE);
Ticker interruptSwingSpeed;

int resetSwingSpeed = 0;

inline void countSwingSpeed() {
    static int i = 0;
    if (resetSwingSpeed == 1) {
        i = 0;
        resetSwingSpeed = 0;
    }
    if (i <= swingspeed) {
        i+=swingspeed/10.0;
        targSwingRadVelocity=i;
    }
    else {
        interruptSwingSpeed.detach(); 
    }
}


inline void initializeSwing() {
    interrupter.rise(&countSwing);
    Motor_swing.period_us(SWING_PERIOD);
    //controller set
    contSwing.setInputLimits(0.0, 100.0);
    contSwing.setOutputLimits(0.0, 1.0);
    contSwing.setBias(0.0);
    contSwing.setMode(AUTO_MODE);
}
float waitTime = 0.0;
void countSwing() {
    Indicator1 = !Indicator1;
    if(enableShoot){
        enableShoot=0;
//        waitTime = (float)(((2.0 * PI) / swingRadVelocity) / 2.0);
//        waitTime = (float)((((10.0 * PI) / 4.0) / swingRadVelocity) / 8.0);
        if(swingRadVelocity==0.0) waitTime = (float)((((2.0 * PI) / swingRadVelocity) / 4.0));//-0.0017);
        else waitTime=0.0;
        if(waitTime<0.0) waitTime=0.0;
        if(waitTime>10.0) waitTime=10.0;
        wait(waitTime);
        sendData(1,6);
    }
}

double swingSita=0.0;
inline void mesureSwing() {
    PulsesSwing       = ( double )SwingSens.getPulses();
    swingRadVelocity  = (((PulsesSwing - PrefPulsesSwing) / 3000.0) * 2.0 * PI) / RATE;
    PrefPulsesSwing   = PulsesSwing;
}

double cont=0.0;
inline void swingFollowing(){
    contSwing.setSetPoint(( float )targSwingRadVelocity);
    contSwing.setProcessValue(( float )swingRadVelocity);
    cont = contSwing.compute();
    Motor_swing = cont;
}

#endif /*Swing.h*/