File content as of revision 4:bf278ddb8504:

#ifndef _LIMIT_H_
#define _LIMIT_H_

#define limit_debug_print false
#define start_duty 200
#define limit_max_power 500

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;

int limit_count1=0;
int limit_count2=0;
int limit_count3=0;
int limit_count4=0;
int limit_count5=0;
int limit_count6=0;

int limit_result1=1;
int limit_result2=1;
int limit_result3=1;
int limit_result4=1;
int limit_result5=1;
int limit_result6=1;

void limit_1()
{
        wait_us(30);
    if(limit_sw1==0)
        limit_count1++;
    else
        limit_count1--;
        
    if(limit_count1>50)
        limit_count1=50;
    if(limit_count1<-50)
        limit_count1=-50;
        
    if(limit_count1>15)
        limit_result1=0;
    if(limit_count1<-15)
        limit_count1=1;
}

void limit_2()
{
        wait_us(30);
    if(limit_sw2==0)
        limit_count2++;
    else
        limit_count2--;
        
    if(limit_count2>50)
        limit_count2=50;
    if(limit_count2<-50)
        limit_count2=-50;
        
    if(limit_count2>15)
        limit_result2=0;
    if(limit_count2<-15)
        limit_count2=1;
}

void limit_3()
{
        wait_us(30);
    if(limit_sw3==0)
        limit_count3++;
    else
        limit_count3--;
        
    if(limit_count3>50)
        limit_count3=50;
    if(limit_count3<-50)
        limit_count3=-50;
        
    if(limit_count3>15)
        limit_result3=0;
    if(limit_count3<-15)
        limit_count3=1;
}


void limit_4()
{
        wait_us(30);
    if(limit_sw4==0)
        limit_count4++;
    else
        limit_count4--;
        
    if(limit_count4>50)
        limit_count4=50;
    if(limit_count4<-50)
        limit_count4=-50;
        
    if(limit_count4>15)
        limit_result4=0;
    if(limit_count4<-15)
        limit_count4=1;
}


void limit_5()
{
        wait_us(30);
    if(limit_sw5==0)
        limit_count5++;
    else
        limit_count5--;
        
    if(limit_count5>50)
        limit_count5=50;
    if(limit_count5<-50)
        limit_count5=-50;
        
    if(limit_count5>15)
        limit_result5=0;
    if(limit_count5<-15)
        limit_count5=1;
}


void limit_6()
{
        wait_us(30);
    if(limit_sw6==0)
        limit_count6++;
    else
        limit_count6--;
        
    if(limit_count6>50)
        limit_count6=50;
    if(limit_count6<-50)
        limit_count6=-50;
        
    if(limit_count6>15)
        limit_result6=0;
    if(limit_count6<-15)
        limit_count6=1;
}



bool limit_check()
{        
    limit_1();
    limit_2();
    limit_3();
    limit_4();
    limit_5();
    limit_6();

    int sw1 = limit_result1;
    int sw2 = limit_result2;
    int sw3 = limit_result3;
    int sw4 = limit_result4;
    int sw5 = limit_result5;
    int sw6 = limit_result6;
    
    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]>limit_max_power)
            limit_result[i]=limit_max_power;
        if(limit_result[i]<-limit_max_power)
            limit_result[i]=-limit_max_power;
        
        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]>limit_max_power)
            limit_result[i]=limit_max_power;
        if(limit_result[i]<-limit_max_power)
            limit_result[i]=-limit_max_power;
        
        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]>limit_max_power)
            limit_result[i]=limit_max_power;
        if(limit_result[i]<-limit_max_power)
            limit_result[i]=-limit_max_power;
        
        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()
{
    bool limit_sucess=false;
    
    wait_ms(0);
    
    encoder_reset_cnt();
    
    for(int i=0;i<limit_time;i++)
    {
        if(limit_check())
        {
            limit_sucess=true;
            break;
        }
        
        if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1)
            {
                while(1)
                {
                    motor_power(0,0);
                    motor_power(1,0);
                    motor_power(2,0);
                    motor_power(3,0);
                    motor_power(4,0);
                    motor_power(5,0);
    
                    pc.printf("encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1\r\n");
                    //encoder_data[0] = encoder1.read();
                    //if(encoder_data[0]!=-1)
                    //{
                        //NVIC_SystemReset();
                    //}
                    wait_us(2500);
                }
            }
            
        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);
    
    while(!limit_sucess)
    {
        pc.printf("Limit-faild\r\n");
    }
    
}

#endif