Diff: limit.h

Revision:

0:7cff999a7f5c

Child:

1:7b5469bf5994

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/limit.h	Mon May 11 08:47:18 2020 +0000
@@ -0,0 +1,313 @@
+#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