lee sanghun / Mbed 2 deprecated 202001131705

45

Dependencies:   mbed BufferedSerial LS7366LIB FastPWM

limit.h@0:c21936a3520a, 2020-03-23 (annotated)

Committer:
lsh2205
Date:
Mon Mar 23 08:38:40 2020 +0000
Revision:
0:c21936a3520a
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:c21936a3520a 1 DigitalIn limit_sw1(LIMIT_SW1);
lsh2205 0:c21936a3520a 2 DigitalIn limit_sw2(LIMIT_SW2);
lsh2205 0:c21936a3520a 3 DigitalIn limit_sw3(LIMIT_SW3);
lsh2205 0:c21936a3520a 4 DigitalIn limit_sw4(LIMIT_SW4);
lsh2205 0:c21936a3520a 5 DigitalIn limit_sw5(LIMIT_SW5);
lsh2205 0:c21936a3520a 6 DigitalIn limit_sw6(LIMIT_SW6);
lsh2205 0:c21936a3520a 7
lsh2205 0:c21936a3520a 8 #define limit_time 10000
lsh2205 0:c21936a3520a 9
lsh2205 0:c21936a3520a 10 bool limit_find[6]={false,false,false,false,false,false};
lsh2205 0:c21936a3520a 11
lsh2205 0:c21936a3520a 12 void limit_init()
lsh2205 0:c21936a3520a 13 {
lsh2205 0:c21936a3520a 14 limit_sw1.mode(PullUp);
lsh2205 0:c21936a3520a 15 limit_sw2.mode(PullUp);
lsh2205 0:c21936a3520a 16 limit_sw3.mode(PullUp);
lsh2205 0:c21936a3520a 17 limit_sw4.mode(PullUp);
lsh2205 0:c21936a3520a 18 limit_sw5.mode(PullUp);
lsh2205 0:c21936a3520a 19 limit_sw6.mode(PullUp);
lsh2205 0:c21936a3520a 20 }
lsh2205 0:c21936a3520a 21
lsh2205 0:c21936a3520a 22 bool limit_check123()
lsh2205 0:c21936a3520a 23 {
lsh2205 0:c21936a3520a 24 limit_sw1.mode(PullUp);
lsh2205 0:c21936a3520a 25 limit_sw2.mode(PullUp);
lsh2205 0:c21936a3520a 26 limit_sw3.mode(PullUp);
lsh2205 0:c21936a3520a 27
lsh2205 0:c21936a3520a 28 int sw1 = limit_sw1;
lsh2205 0:c21936a3520a 29 int sw2 = limit_sw2;
lsh2205 0:c21936a3520a 30 int sw3 = limit_sw3;
lsh2205 0:c21936a3520a 31
lsh2205 0:c21936a3520a 32 //pc.printf("Limit state : %d , %d , %d ",sw1,sw2,sw3);
lsh2205 0:c21936a3520a 33 //pc.printf("\n");
lsh2205 0:c21936a3520a 34
lsh2205 0:c21936a3520a 35
lsh2205 0:c21936a3520a 36 if(limit_find[0]==false)
lsh2205 0:c21936a3520a 37 motor_power(0,-150);
lsh2205 0:c21936a3520a 38 if(limit_find[1]==false)
lsh2205 0:c21936a3520a 39 motor_power(1,-150);
lsh2205 0:c21936a3520a 40 if(limit_find[2]==false)
lsh2205 0:c21936a3520a 41 motor_power(2,-150);
lsh2205 0:c21936a3520a 42
lsh2205 0:c21936a3520a 43 if(sw1==0 && !limit_find[0])
lsh2205 0:c21936a3520a 44 {
lsh2205 0:c21936a3520a 45 limit_find[0]=true;
lsh2205 0:c21936a3520a 46 //pc.printf("find_limit : axis [ 1 ] ");
lsh2205 0:c21936a3520a 47 //pc.printf("\n");
lsh2205 0:c21936a3520a 48 motor_power(0,0);
lsh2205 0:c21936a3520a 49 encoder1.LS7366_reset_counter();
lsh2205 0:c21936a3520a 50 }
lsh2205 0:c21936a3520a 51
lsh2205 0:c21936a3520a 52 if(sw2==0 && !limit_find[1])
lsh2205 0:c21936a3520a 53 {
lsh2205 0:c21936a3520a 54 limit_find[1]=true;
lsh2205 0:c21936a3520a 55 //pc.printf("find_limit : axis [ 2 ] ");
lsh2205 0:c21936a3520a 56 //pc.printf("\n");
lsh2205 0:c21936a3520a 57 motor_power(1,0);
lsh2205 0:c21936a3520a 58 encoder2.LS7366_reset_counter();
lsh2205 0:c21936a3520a 59 }
lsh2205 0:c21936a3520a 60
lsh2205 0:c21936a3520a 61 if(sw3==0 && !limit_find[2])
lsh2205 0:c21936a3520a 62 {
lsh2205 0:c21936a3520a 63 limit_find[2]=true;
lsh2205 0:c21936a3520a 64 //pc.printf("find_limit : axis [ 3 ] ");
lsh2205 0:c21936a3520a 65 //pc.printf("\n");
lsh2205 0:c21936a3520a 66 motor_power(2,0);
lsh2205 0:c21936a3520a 67 encoder3.LS7366_reset_counter();
lsh2205 0:c21936a3520a 68 }
lsh2205 0:c21936a3520a 69
lsh2205 0:c21936a3520a 70
lsh2205 0:c21936a3520a 71
lsh2205 0:c21936a3520a 72 return limit_find[0] && limit_find[1] && limit_find[2];
lsh2205 0:c21936a3520a 73 }
lsh2205 0:c21936a3520a 74
lsh2205 0:c21936a3520a 75 void find_limit()
lsh2205 0:c21936a3520a 76 {
lsh2205 0:c21936a3520a 77 wait_ms(100);
lsh2205 0:c21936a3520a 78 //pc.printf("find_limit_start~~");
lsh2205 0:c21936a3520a 79 //pc.printf("\n");
lsh2205 0:c21936a3520a 80 for(int i=0;i<limit_time;i++)
lsh2205 0:c21936a3520a 81 {
lsh2205 0:c21936a3520a 82 if(limit_check123())
lsh2205 0:c21936a3520a 83 {
lsh2205 0:c21936a3520a 84 break;
lsh2205 0:c21936a3520a 85 }
lsh2205 0:c21936a3520a 86 wait_ms(1);
lsh2205 0:c21936a3520a 87 /*
lsh2205 0:c21936a3520a 88 if(i==limit_time)
lsh2205 0:c21936a3520a 89 {
lsh2205 0:c21936a3520a 90 pc.printf("Can not find LIMIT !!!");
lsh2205 0:c21936a3520a 91 pc.printf("\n");
lsh2205 0:c21936a3520a 92 }
lsh2205 0:c21936a3520a 93 */
lsh2205 0:c21936a3520a 94 }
lsh2205 0:c21936a3520a 95
lsh2205 0:c21936a3520a 96 }