Diff: limit.h

Revision:

0:c21936a3520a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/limit.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,96 @@
+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
+
+bool limit_find[6]={false,false,false,false,false,false};
+
+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);
+}
+
+bool limit_check123()
+{
+    limit_sw1.mode(PullUp);
+    limit_sw2.mode(PullUp);
+    limit_sw3.mode(PullUp);
+    
+    int sw1 = limit_sw1;
+    int sw2 = limit_sw2;
+    int sw3 = limit_sw3;
+
+    //pc.printf("Limit state : %d , %d , %d ",sw1,sw2,sw3);
+    //pc.printf("\n");
+    
+    
+    if(limit_find[0]==false)
+        motor_power(0,-150);
+    if(limit_find[1]==false)
+        motor_power(1,-150);
+    if(limit_find[2]==false)
+        motor_power(2,-150);
+    
+    if(sw1==0 && !limit_find[0])
+    {
+    limit_find[0]=true;
+    //pc.printf("find_limit : axis [ 1 ] ");
+    //pc.printf("\n");
+    motor_power(0,0);
+    encoder1.LS7366_reset_counter();
+    }
+    
+    if(sw2==0 && !limit_find[1])
+    {
+    limit_find[1]=true;
+    //pc.printf("find_limit : axis [ 2 ] ");
+    //pc.printf("\n");
+    motor_power(1,0);
+    encoder2.LS7366_reset_counter();
+    }
+
+    if(sw3==0 && !limit_find[2])
+    {
+    limit_find[2]=true;
+    //pc.printf("find_limit : axis [ 3 ] ");
+    //pc.printf("\n");
+    motor_power(2,0);
+    encoder3.LS7366_reset_counter();
+    }
+
+    
+    
+    return limit_find[0] && limit_find[1] && limit_find[2];
+}
+
+void find_limit()
+{
+    wait_ms(100);
+    //pc.printf("find_limit_start~~");
+    //pc.printf("\n");
+    for(int i=0;i<limit_time;i++)
+    {
+        if(limit_check123())
+        {
+            break;
+        }
+        wait_ms(1);
+        /*
+        if(i==limit_time)
+        {
+            pc.printf("Can not find LIMIT !!!");
+            pc.printf("\n");
+        }
+        */
+    }   
+        
+}
\ No newline at end of file