control_signal += A[0] * error[0] + A[1] * error[1] + A[2] * error[2];

if( control_signal < 0 ) {
    if( control_signal < -(4000 << 16) )
        control_signal = -(4000 << 16);
    EPWM1A = (-control_signal) >> 16;
    gpio_set_one_low( &GPIO0, 31 );
} else {
    if( control_signal > (4000 << 16) )
        control_signal = (4000 << 16);
    EPWM1A = control_signal >> 16;
    gpio_set_one_high( &GPIO0, 31 );
}