File content as of revision 12:58ad06f9847d:

#include "sm_servo.h"

PwmOut pwm_Servo(PE_9);

// declaration des entrées analogiques
AnalogIn front(PA_5);
AnalogIn ana_left_90(PA_4);
AnalogIn ana_right_90(PF_10);
AnalogIn ana_left_45(PC_0);
AnalogIn ana_right_45(PF_4);
AnalogIn ana_left_5(PF_9);
AnalogIn ana_right_5(PA_6);

AnalogIn analog_value_PF3(PF_3);

E_STATE_SERVO e_stateServo;
bool directionCheck = false;
int pulsewidth = SERVO_PULSE_MIDDLE_US;//SERVO_PULSE_MAX_US;

double d_last_odom_asservissement = 0;
int i_lastTimeAsserv;

double d_prev_dist_left_90;
double d_prev_dist_right_90;
double d_dist_left_90;
double d_dist_right_90;

double d_prev_dist_left_45;
double d_prev_dist_right_45;
double d_dist_left_45;
double d_dist_right_45;

double d_positionOnTrack;

double coef_positionnement;
double coef_angle_correction_bordure;
double angle_correction_final ;
double angle_correction_position;
double angle_correction_bordure;
double distance_parcourue;

double ponderation_angle[6];
double mask_table[6];
double d_mediane;
double d_moyenne;
double d_sumMask;
double distCapt;
double d_SERVO_coefFreinage = 0.0;

void init_sm_servo()
{
    e_stateServo = SERVO_INIT;
}

double pwmFromAngle(double angleDeg)
{
    //on a une regression linéaire entre l'angle et la pwm on centre sur 0 puis on applique
    pulsewidth = (d_CHASSIS_inversion*angleDeg * 11.0) + SERVO_PULSE_MIDDLE_US;
    //rs_LOG_pc.printf("pulsewidth = %d =(%f * 11.0) + 1500\r\n",pulsewidth,angleDeg);
    if(pulsewidth > SERVO_PULSE_MAX_US) {
        pulsewidth =  SERVO_PULSE_MAX_US;
    } else if(pulsewidth < SERVO_PULSE_MIN_US) {
        pulsewidth = SERVO_PULSE_MIN_US;
    }

    return pulsewidth;
}



double getDistCapteur(AnalogIn* p)
{
    distCapt = 0.182/(double)p->read() ;
    if (distCapt > 1.5) {
        distCapt = 1.5 ;
    }
    return distCapt ;
}

//renvoie un double entre 0 et 1 avec 0 position extrème gauche et 1 extrème droite
double positionOnTrack()
{
    d_positionOnTrack = 0.25*(3*(d_dist_left_90/(d_dist_left_90 + d_dist_right_90)) + (d_dist_left_45/(d_dist_left_45 + d_dist_right_45) ));

    return d_positionOnTrack;
}

double compute_angle_correction(double consignePos)
{
    if(t_utils_timerSinceStart.read_ms() - i_lastTimeAsserv  > 40){
    //aquisition
    coef_positionnement = s_UTILS_currentSection->coef_pos;
    coef_angle_correction_bordure = s_UTILS_currentSection->coef_bord;

    d_dist_left_90 = getDistCapteur(&ana_left_90) + 0.016;
    d_dist_right_90 = getDistCapteur(&ana_right_90) + 0.016;
    d_dist_left_45 = getDistCapteur(&ana_left_45) + 0.042;
    d_dist_right_45 = getDistCapteur(&ana_right_45) + 0.042;

    angle_correction_final = 0.0;
    angle_correction_position = 0.0;
    angle_correction_bordure = 0.0;

    update_speed();

    distance_parcourue = d_ODOM_computed_pos_m - d_last_odom_asservissement ;

    //left 90
    ponderation_angle[0] = atan2( (d_prev_dist_left_90 - d_dist_left_90), distance_parcourue)*57.296 ;
    //right 90
    ponderation_angle[1] = -atan2( (d_prev_dist_right_90 - d_dist_right_90), distance_parcourue)*57.296 ;
    //left 45
    ponderation_angle[2] = atan2( ((d_prev_dist_left_45 - d_dist_left_45)*0.7071), distance_parcourue)*57.296 ;
    //right 45
    ponderation_angle[3] = -atan2( ((d_prev_dist_right_45 - d_dist_right_45)*0.7071), distance_parcourue)*57.296 ;

    //angle detecte entre le 45 et le 90 gauche
    ponderation_angle[4] = atan(1.4142*(d_prev_dist_left_90/d_prev_dist_left_45) - 1)*57.296;

    //angle detecte entre le 45 et le 90 droite
    ponderation_angle[5] = -atan(1.4142*(d_prev_dist_right_90/d_prev_dist_right_45) - 1)*57.296;

    s_LOG_history[i_LOG_index_data].angle_bord0 = ponderation_angle[0];
    s_LOG_history[i_LOG_index_data].angle_bord1 = ponderation_angle[1];
    s_LOG_history[i_LOG_index_data].angle_bord2 = ponderation_angle[2];
    s_LOG_history[i_LOG_index_data].angle_bord3 = ponderation_angle[3];
    s_LOG_history[i_LOG_index_data].angle_bord4 = ponderation_angle[4];
    s_LOG_history[i_LOG_index_data].angle_bord5 = ponderation_angle[5];
    s_LOG_history[i_LOG_index_data].distParcourue = distance_parcourue;

    //votation
    if(d_ODOM_speed_mps < 50.0 && d_ODOM_speed_mps > 0.01)
    {
        bubbleSort(ponderation_angle,6);
        d_mediane = (ponderation_angle[2] + ponderation_angle[3])*0.5;
    }
    else
    {
        d_mediane = (ponderation_angle[4] + ponderation_angle[5])*0.5; 
    }

    angle_correction_bordure = d_mediane;

    angle_correction_position = (consignePos - positionOnTrack());

    //rs_LOG_pc.printf("distance_parcourue = %f = %f - %f + 0.001 \n\r",distance_parcourue,d_ODOM_computed_pos_m,d_last_odom_asservissement );
    //rs_LOG_pc.printf("angle_correction_bordure = %f = -atan((%f - %f)/ %f) \n\r",angle_correction_bordure,d_prev_dist_left_90,d_dist_left_90,distance_parcourue) ;

    angle_correction_final = angle_correction_position * coef_positionnement + angle_correction_bordure*coef_angle_correction_bordure ;

    d_last_odom_asservissement = d_ODOM_computed_pos_m;
    d_prev_dist_left_90 = d_dist_left_90;
    d_prev_dist_right_90 = d_dist_right_90;
    d_prev_dist_left_45 = d_dist_left_45;
    d_prev_dist_right_45 = d_dist_right_45;


    //logging
    s_LOG_history[i_LOG_index_data].pwm_dir = pulsewidth;
    s_LOG_history[i_LOG_index_data].left_90 = d_dist_left_90;
    s_LOG_history[i_LOG_index_data].right_90 = d_dist_right_90;
    s_LOG_history[i_LOG_index_data].left_45 = d_dist_left_45;
    s_LOG_history[i_LOG_index_data].right_45 = d_dist_right_45;
    s_LOG_history[i_LOG_index_data].odom = d_ODOM_distFromGlobalStart_m;
    s_LOG_history[i_LOG_index_data].speed = d_ODOM_speed_mps;

    s_LOG_history[i_LOG_index_data].angle_pos = angle_correction_position;
    s_LOG_history[i_LOG_index_data].angle = angle_correction_final;

    log_check();

    
    d_SERVO_coefFreinage = (abs(angle_correction_final)/90);
    i_lastTimeAsserv = t_utils_timerSinceStart.read_ms();
    }
    return  angle_correction_final;
}


void update_sm_servo()
{
    E_STATE_SERVO next_state = e_stateServo;

    switch(e_stateServo) {
        case SERVO_INIT:
            if(directionCheck) {
                next_state = SERVO_COMMAND;
            }
            break;
        case SERVO_COMMAND:
            //si on detecte une nouvelle acquisition, on traite le signal
            d_dist_left_90 = getDistCapteur(&ana_left_90);
            //rs_LOG_pc.printf("d_dist_left_90 : %f;d_dist_right_90 : %f;d_positionOnTrack: %f\r\n",d_dist_left_90,d_dist_right_90,d_positionOnTrack);
            if(d_dist_left_90 != d_prev_dist_left_90) {

                //rs_LOG_pc.printf("compute angle et updateSpeed\r\n");
                pulsewidth = pwmFromAngle( compute_angle_correction(s_UTILS_currentSection->consigne_position) );

                d_prev_dist_left_90 = d_dist_left_90;

            }


            break;
        default:
            break;
    }

    e_stateServo = next_state;
    return;
}

void output_sm_servo()
{
    switch(e_stateServo) {
        case SERVO_INIT:
            rs_LOG.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
            pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
            pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
            directionCheck = true;
            break;
        case SERVO_COMMAND:
            pwm_Servo.pulsewidth_us(pulsewidth);

            break;
        default:
            break;
    }
    return;
}