File content as of revision 4:bf278ddb8504:

#ifndef _ENCODER_H_
#define _ENCODER_H_

int ex_encoder_data[6]={0,};
int dif_encoder_data[6]={0,};
double filter_dif_encoder_data[6]={0,};
double filter_dif_encoder_co[6] = {0.01,0.01,0.01,0.01,0.01,0.01};

void filter_encoder_data()
{
    for(int i=0; i<6;i++)
    {
        dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i];
        
        filter_dif_encoder_data[i] = filter_dif_encoder_data[i]*(1-filter_dif_encoder_co[i]) + (double)dif_encoder_data[i]*filter_dif_encoder_co[i];
        
        ex_encoder_data[i] = encoder_data[i];
    }
}

void encoder_read_raw()
{
    encoder_data[0] = encoder1.read();
    encoder_data[1] = encoder2.read();
    encoder_data[2] = encoder3.read();
    encoder_data[3] = encoder4.read();
    encoder_data[4] = -encoder5.read();
    encoder_data[5] = -encoder6.read();
    
}


void encoder_read()
{
    encoder_data[0] = encoder1.read();
    encoder_data[1] = encoder2.read();
    encoder_data[2] = encoder3.read();
    encoder_data[3] = encoder4.read();
    encoder_data[4] = -encoder5.read();
    encoder_data[5] = -encoder6.read();
    
    /*
    for(int i=0;i<6;i++)
        {
            pc.printf("%8d", encoder_data[i]);
        }pc.printf("\r\n");
    */
    filter_encoder_data();
}

/*
void reset_check()
{
    }
*/
void reset_check()
{
}

/*
            encoder_data[0] = encoder1.read();
            if(encoder_data[0]!=-1)
            {
                NVIC_SystemReset();
            }
*/
        

void encoder_reset_cnt()
{
    encoder1.LS7366_reset_counter();
    encoder2.LS7366_reset_counter();
    encoder3.LS7366_reset_counter();
    encoder4.LS7366_reset_counter();
    encoder5.LS7366_reset_counter();
    encoder6.LS7366_reset_counter();
}


void Time_To_Motor_Drive(int dir, double time_ms)
{
    const int motor_percent[6] = {400, 400, 400, 350, -200, -200};
    int motor_4_dir = 1;
    
    for(int i = 0; i < 6; i++)
    {
        if(i == 3)
        {
            motor_4_dir = -1;
        }
        else
        {
            motor_4_dir = 1;
        }
        motor_power(i, motor_percent[i] * dir * motor_4_dir);
    }
    wait_ms(time_ms);
    
    if(dir == -1)
    {
        for(int i = 4; i < 6; i++)
        {
            motor_power(i, 0);
        }
        wait_ms(time_ms);
    }
}


void encoder_check0()
{
    
    int forward_enc[6] = {0,};
    int reverse_enc[6] = {0,};
    bool encoder_state = false;
        
    Time_To_Motor_Drive(0, 200);
    encoder_reset_cnt();
    encoder_read();  
    
    for(int i = 0; i < 4; i++)
    {
        if(encoder_data[i] == -1)
        {
            encoder_state = true;
            pc.printf("%d : -1 \n\r", i);
        }
    }
    for(int i = 4; i < 6; i++)
    {
        if(encoder_data[i] == 1)
        {
            encoder_state = true;
            pc.printf("%d : -1 \n\r", i);
        }
    }
    
    if(encoder_state != true)
    {
        Time_To_Motor_Drive(1, 200);
        Time_To_Motor_Drive(0, 1000);
        encoder_read();
        
        for(int i = 0; i < 6; i++)
        {
            forward_enc[i] = encoder_data[i];// * -1;
        }
        Time_To_Motor_Drive(-1, 250);
        Time_To_Motor_Drive(0, 1000);
        encoder_read();
        
        for(int i = 0; i < 6; i++)
        {
            reverse_enc[i] = encoder_data[i];// * -1;
            if((forward_enc[i] > 100)  || (forward_enc[i] > reverse_enc[i]) || (forward_enc[i] == 0 && reverse_enc[i] == 0))
            {
                encoder_state = true;
                pc.printf("%d ", i);
            }
        }
    }
    
    //error
    if(encoder_state == true)
    {
        pc.printf("Encoder error!\n\r");
        wait_ms(1000);
        while(1)
        {
        }
    }
}

void encoder_check2()
{
    encoder_reset_cnt();
    
    
    encoder_read(); 
    
    for(int i=0;i<6;i++)
        {
            pc.printf("%8d", encoder_data[i]);
        }pc.printf("\r\n");
        
    int encoder_check_move[6]={0,0,0,0,0,0};
        
    for(int i=0; i<200;i++)
    {
        encoder_read(); 
        
        for(int k=0;k<6;k++)
        {
            if(encoder_check[k]==false)
            {
                if(encoder_data[k]<-3)
                    encoder_check[k]=true;
                if(encoder_data[k]>3)
                    encoder_check[k]=true;
                encoder_check_move[k]=i;
            }
                
                
            if(encoder_check[k])
            {
               motor_power(k, 0); 
            }
            else
            {
                motor_power(k, i);
            }
        }
    }
    
    
    for(int i=0;i<6;i++)
    motor_power(i, 0);
    
    wait_ms(100);
    
     for(int i=0;i<6;i++)
        {
            pc.printf("%8d", encoder_data[i]);
        }pc.printf("\r\n");
    
    for(int i=0; i<200;i++)
    {
        encoder_read(); 
        for(int k=0;k<6;k++)
        {
            if(encoder_check[k]==false)
                motor_power(k, -i);
        }
    }
    
    for(int i=0;i<6;i++)
    motor_power(i, 0);
    

    wait_ms(100);
    
    encoder_read(); 
    for(int i=0;i<6;i++)
        {
            pc.printf("%8d", encoder_data[i]);
        }pc.printf("\r\n");
    for(int i=0;i<6;i++)
        {
            pc.printf("%8d", encoder_check_move[i]);
        }pc.printf("\r\n");
        
    for(int i=0;i<6;i++)
        {
            if(encoder_check[i])
                pc.printf("      OK");
            else
                pc.printf("      ER");
                
                motor_onoff[i]=encoder_check[i];
        }pc.printf("\r\n");
    
    
    
}

#endif