File content as of revision 0:e12eb40b9fef:

#ifndef _LIMIT_H_
#define _LIMIT_H_

#define limit_debug_print false
#define start_duty 200

bool limit_find[6]={false,false,false,false,false,false};

DigitalIn limit_sw1(LIMIT_SW1);
DigitalIn limit_sw2(LIMIT_SW2);
DigitalIn limit_sw3(LIMIT_SW3);
DigitalIn limit_sw4(LIMIT_SW4);
DigitalIn limit_sw5(LIMIT_SW5);
DigitalIn limit_sw6(LIMIT_SW6);

#define limit_time 10000

void limit_init()
{
    limit_sw1.mode(PullUp);
    limit_sw2.mode(PullUp);
    limit_sw3.mode(PullUp);
    limit_sw4.mode(PullUp);
    limit_sw5.mode(PullUp);
    limit_sw6.mode(PullUp);
}


double limit_ex_encoder_data[6]={0,};
double diff_encoder_data[6]={0,0,0,0,0,0};
double sum_error[6]={0,0,0,0,0,0};
double motor_duty[6]={-60,0,0,0,0,0};
double limit_taget_speed[6]={0,0,0,0,0,0};
double limit_result[6]={0,0,0,0,0,0};
double limit_output[6]={0,0,0,0,0,0};
double limit_error[6]={0,0,0,0,0,0};

double axis123_taget_speed=30;
double axis4_taget_speed=-56;
double axis56_taget_speed=30;

int ccnt=0;
bool limit_check()
{        

    int sw1 = limit_sw1;
    int sw2 = limit_sw2;
    int sw3 = limit_sw3;
    int sw4 = limit_sw4;
    int sw5 = limit_sw5;
    int sw6 = limit_sw6;
    
    encoder_read_raw();
    

    if(limit_find[0]==false)
    {
        if(limit_taget_speed[0]<axis123_taget_speed)
            limit_taget_speed[0]+=0.3;
            
            motor_power(0,limit_output[0]);
    }else
    {
       if(limit_taget_speed[0]>0)
            limit_taget_speed[0]-=1;
            
       if(limit_taget_speed[0]<0)
            {
                limit_taget_speed[0]=0;   
            }
         motor_power(0,limit_output[0]);
    }
    
    if(limit_find[1]==false)
    {
        if(limit_taget_speed[1]<axis123_taget_speed)
            limit_taget_speed[1]+=0.3;
            
            motor_power(1,limit_output[1]);
    }else
    {
       if(limit_taget_speed[1]>0)
            limit_taget_speed[1]-=1;
            
       if(limit_taget_speed[1]<0)
            {
                limit_taget_speed[1]=0;   
            } 
        motor_power(1,limit_output[1]);
    }
    
    if(limit_find[2]==false)
    {
        if(limit_taget_speed[2]<axis123_taget_speed)
            limit_taget_speed[2]+=0.3;
            
            motor_power(2,limit_output[2]);
    }else
    {
       if(limit_taget_speed[2]>0)
            limit_taget_speed[2]-=1;
            
       if(limit_taget_speed[2]<0)
            {
                limit_taget_speed[2]=0;   
            }
        motor_power(2,limit_output[2]);
    }
    
    if(limit_find[3]==false)
    {
        if(limit_taget_speed[3]>axis4_taget_speed)
            limit_taget_speed[3]-=0.3;
            
            motor_power(3,limit_output[3]);
    }else
    {
       if(limit_taget_speed[3]<0)
            limit_taget_speed[3]+=1;
            
       if(limit_taget_speed[3]>0)
            {
                limit_taget_speed[3]=0;   
            }   
            motor_power(3,limit_output[3]);
    }
    
    if(ccnt<450)
    {
        ccnt++;
        limit_taget_speed[0]=0;
        limit_taget_speed[1]=0;
        limit_taget_speed[2]=0;
        limit_taget_speed[3]=0;
        limit_taget_speed[4]-=0.4;
        limit_taget_speed[5]-=0.4;
    }

    if(limit_find[4]==false)
    {
        if(limit_taget_speed[4]<axis56_taget_speed)
            limit_taget_speed[4]+=0.3;
            
            motor_power(4,-limit_output[4]);
    }else
    {
            if(limit_taget_speed[4]>0)
            limit_taget_speed[4]-=1;
            
       if(limit_taget_speed[4]<0)
            {
                limit_taget_speed[4]=0;   
            }
            motor_power(4,-limit_output[4]);  
    }
    if(limit_find[5]==false)
    {
        if(limit_taget_speed[5]<axis56_taget_speed)
            limit_taget_speed[5]+=0.3;
            
            motor_power(5,-limit_output[5]);
    }else
    {
            if(limit_taget_speed[5]>0)
            limit_taget_speed[5]-=1;
            
       if(limit_taget_speed[5]<0)
            {
                limit_taget_speed[5]=0;   
            } 
            motor_power(5,-limit_output[5]);
    }
    

    for(int i=0;i<3;i++)
    {
    
        diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
        limit_ex_encoder_data[i]=encoder_data[i];
        limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
        sum_error[i]+=limit_error[i]*0.2;
        limit_result[i]=limit_error[i]*2+sum_error[i]*1;
        
        if(limit_result[i]>200)
            limit_result[i]=200;
        if(limit_result[i]<-200)
            limit_result[i]=-200;
        
        limit_output[i]=motor_duty[i]+limit_result[i];
        
        if(limit_debug_print)
            pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
    }
    
    
    for(int i=3;i<4;i++)
    {
    
        diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
        limit_ex_encoder_data[i]=encoder_data[i];
        limit_error[i]=-(diff_encoder_data[i]-limit_taget_speed[i]);
        sum_error[i]+=limit_error[i]*0.2;
        limit_result[i]=limit_error[i]*2+sum_error[i]*1;
        
        if(limit_result[i]>200)
            limit_result[i]=200;
        if(limit_result[i]<-200)
            limit_result[i]=-200;
        
        limit_output[i]=motor_duty[i]+limit_result[i];

        if(limit_debug_print)
            pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
    }
    
    
    for(int i=4;i<6;i++)
    {
    
        diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
        limit_ex_encoder_data[i]=encoder_data[i];
        limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
        sum_error[i]+=limit_error[i]*0.2;
        limit_result[i]=limit_error[i]*2+sum_error[i]*1;
        
        if(limit_result[i]>200)
            limit_result[i]=200;
        if(limit_result[i]<-200)
            limit_result[i]=-200;
        
        limit_output[i]=motor_duty[i]+limit_result[i];
        
        if(limit_debug_print)
            pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
    }
    
    
    if(limit_debug_print)
        pc.printf("\n\r");
        
        
    
    if(sw1==0 && !limit_find[0])
    {
        limit_find[0]=true;
        encoder1.LS7366_reset_counter();
        limit_ex_encoder_data[0]=0;
    }
    
    if(sw2==0 && !limit_find[1])
    {
        limit_find[1]=true;
        encoder2.LS7366_reset_counter();
        limit_ex_encoder_data[1]=0;
    }

    if(sw3==0 && !limit_find[2])
    {
        limit_find[2]=true;
        encoder3.LS7366_reset_counter();
        limit_ex_encoder_data[2]=0;
    }

    if(sw4==0 && !limit_find[3])
    {
        limit_find[3]=true;
        encoder4.LS7366_reset_counter();
        limit_ex_encoder_data[3]=0;
    }
    if(sw5==0 && !limit_find[4])
    {
        limit_find[4]=true;
        encoder5.LS7366_reset_counter();
        limit_ex_encoder_data[4]=0;
    }
    if(sw6==0 && !limit_find[5])
    {
        limit_find[5]=true;
        encoder6.LS7366_reset_counter();
        limit_ex_encoder_data[5]=0;
    }
    
    return (limit_find[0] || !motor_onoff[0]) && (limit_find[1] || !motor_onoff[1]) && (limit_find[2] || !motor_onoff[2]) && (limit_find[3] || !motor_onoff[3]) && (limit_find[4] || !motor_onoff[4]) && (limit_find[5] || !motor_onoff[5]);
}

bool limit_check_done=false;
int limit_check_done_cnt=0;
void find_limit()
{
    wait_ms(0);
    
    encoder_reset_cnt();
    
    for(int i=0;i<limit_time;i++)
    {
        if(limit_check())
        {
            break;
        }
        reset_check();
        wait_ms(1);
        
    }
    encoder_read_raw();
    motor_power(0,0);
    motor_power(1,0);
    motor_power(2,0);
    motor_power(3,0);
    motor_power(4,0);
    motor_power(5,0);
}

#endif