speed code
Dependencies: mbed
interupts.h@1:f4e3365155e1, 2015-10-18 (annotated)
- Committer:
- benparkes
- Date:
- Sun Oct 18 18:23:58 2015 +0000
- Revision:
- 1:f4e3365155e1
new code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benparkes | 1:f4e3365155e1 | 1 | |
benparkes | 1:f4e3365155e1 | 2 | int HEPulse(int let,int num, int order) //tell us which hall effect HEA1= hall effect, (let, A=1, B=2), (num, 1/2), order (index possition to be filled alternates) |
benparkes | 1:f4e3365155e1 | 3 | { |
benparkes | 1:f4e3365155e1 | 4 | float fA1; |
benparkes | 1:f4e3365155e1 | 5 | float fA2; |
benparkes | 1:f4e3365155e1 | 6 | float fB1; |
benparkes | 1:f4e3365155e1 | 7 | float fB2; |
benparkes | 1:f4e3365155e1 | 8 | int time; |
benparkes | 1:f4e3365155e1 | 9 | time = timer.read_us(); //use to check for interrupt debounce |
benparkes | 1:f4e3365155e1 | 10 | |
benparkes | 1:f4e3365155e1 | 11 | if((tA1[0]-time<10)||(tA2[0]-time<10)||(tA1[1]-time<10)||(tA2[1]-time<10)) { |
benparkes | 1:f4e3365155e1 | 12 | return 1; |
benparkes | 1:f4e3365155e1 | 13 | } // if less than 10us from last interupt assume debouce and ignore |
benparkes | 1:f4e3365155e1 | 14 | //find right thing to do for current hall effect |
benparkes | 1:f4e3365155e1 | 15 | switch(order) { //check whether to fill index possition 0/1 |
benparkes | 1:f4e3365155e1 | 16 | case(1) : |
benparkes | 1:f4e3365155e1 | 17 | switch(let) { //check hall effect A/B |
benparkes | 1:f4e3365155e1 | 18 | case(1): |
benparkes | 1:f4e3365155e1 | 19 | |
benparkes | 1:f4e3365155e1 | 20 | switch(num) { //check hall effect 1/2 |
benparkes | 1:f4e3365155e1 | 21 | case (1): |
benparkes | 1:f4e3365155e1 | 22 | tA1[0] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 23 | distanceA += (wheelc/6); //increment distance |
benparkes | 1:f4e3365155e1 | 24 | pulse_orderA = 2; //toggle index to be filled |
benparkes | 1:f4e3365155e1 | 25 | break; |
benparkes | 1:f4e3365155e1 | 26 | |
benparkes | 1:f4e3365155e1 | 27 | case(2) : |
benparkes | 1:f4e3365155e1 | 28 | tA2[0] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 29 | pulse_orderA = 2; |
benparkes | 1:f4e3365155e1 | 30 | break; |
benparkes | 1:f4e3365155e1 | 31 | } |
benparkes | 1:f4e3365155e1 | 32 | break; |
benparkes | 1:f4e3365155e1 | 33 | |
benparkes | 1:f4e3365155e1 | 34 | |
benparkes | 1:f4e3365155e1 | 35 | case(2): //check hall effect A/B |
benparkes | 1:f4e3365155e1 | 36 | |
benparkes | 1:f4e3365155e1 | 37 | switch(num) { |
benparkes | 1:f4e3365155e1 | 38 | case (1): |
benparkes | 1:f4e3365155e1 | 39 | tB1[0] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 40 | distanceB += (wheelc/6); |
benparkes | 1:f4e3365155e1 | 41 | pulse_orderB = 2; |
benparkes | 1:f4e3365155e1 | 42 | break; |
benparkes | 1:f4e3365155e1 | 43 | |
benparkes | 1:f4e3365155e1 | 44 | case(2) : |
benparkes | 1:f4e3365155e1 | 45 | tB2[0] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 46 | pulse_orderB = 2; |
benparkes | 1:f4e3365155e1 | 47 | break; |
benparkes | 1:f4e3365155e1 | 48 | } |
benparkes | 1:f4e3365155e1 | 49 | break; |
benparkes | 1:f4e3365155e1 | 50 | |
benparkes | 1:f4e3365155e1 | 51 | } |
benparkes | 1:f4e3365155e1 | 52 | case(2) : |
benparkes | 1:f4e3365155e1 | 53 | switch(let) { |
benparkes | 1:f4e3365155e1 | 54 | case(1): |
benparkes | 1:f4e3365155e1 | 55 | |
benparkes | 1:f4e3365155e1 | 56 | switch(num) { |
benparkes | 1:f4e3365155e1 | 57 | case (1): |
benparkes | 1:f4e3365155e1 | 58 | tA1[1] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 59 | distanceA += (wheelc/6); |
benparkes | 1:f4e3365155e1 | 60 | fA1 = 1.0f/(( fabsf(tA1[1]-tA1[0]) ) * (float)3.0E-6); |
benparkes | 1:f4e3365155e1 | 61 | pulse_orderA = 1; |
benparkes | 1:f4e3365155e1 | 62 | break; |
benparkes | 1:f4e3365155e1 | 63 | |
benparkes | 1:f4e3365155e1 | 64 | case(2) : |
benparkes | 1:f4e3365155e1 | 65 | tA2[1] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 66 | fA2 = 1.0f/(( fabsf(tA2[1]-tA2[0]) ) * (float)3.0E-6); |
benparkes | 1:f4e3365155e1 | 67 | pulse_orderA = 1; |
benparkes | 1:f4e3365155e1 | 68 | break; |
benparkes | 1:f4e3365155e1 | 69 | } |
benparkes | 1:f4e3365155e1 | 70 | break; |
benparkes | 1:f4e3365155e1 | 71 | |
benparkes | 1:f4e3365155e1 | 72 | |
benparkes | 1:f4e3365155e1 | 73 | case(2): |
benparkes | 1:f4e3365155e1 | 74 | |
benparkes | 1:f4e3365155e1 | 75 | switch(num) { |
benparkes | 1:f4e3365155e1 | 76 | case (1): |
benparkes | 1:f4e3365155e1 | 77 | tB1[1] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 78 | distanceB += (wheelc/6); |
benparkes | 1:f4e3365155e1 | 79 | fB1 = 1.0f/(( fabsf(tB1[1]-tB1[0]) ) * (float)3.0E-6); |
benparkes | 1:f4e3365155e1 | 80 | pulse_orderB = 1; |
benparkes | 1:f4e3365155e1 | 81 | break; |
benparkes | 1:f4e3365155e1 | 82 | |
benparkes | 1:f4e3365155e1 | 83 | case(2) : |
benparkes | 1:f4e3365155e1 | 84 | tB2[1] = timer.read_us(); |
benparkes | 1:f4e3365155e1 | 85 | fB2 = 1.0f/((fabsf( tB2[1]-tB2[0]) ) * (float)3.0E-6); |
benparkes | 1:f4e3365155e1 | 86 | pulse_orderB = 1; |
benparkes | 1:f4e3365155e1 | 87 | break; |
benparkes | 1:f4e3365155e1 | 88 | } |
benparkes | 1:f4e3365155e1 | 89 | break; |
benparkes | 1:f4e3365155e1 | 90 | |
benparkes | 1:f4e3365155e1 | 91 | } |
benparkes | 1:f4e3365155e1 | 92 | |
benparkes | 1:f4e3365155e1 | 93 | float fA = (fA1 + fA2)*0.5f; //calculate average frequency of hall effect A |
benparkes | 1:f4e3365155e1 | 94 | float fB = (fB1 + fB2)*0.5f; //calculate average frequency of hall effect B |
benparkes | 1:f4e3365155e1 | 95 | speedA = fA/20.8f; //output speeds of motors by frequency/gear box ratio |
benparkes | 1:f4e3365155e1 | 96 | speedB = fB/20.8f; |
benparkes | 1:f4e3365155e1 | 97 | |
benparkes | 1:f4e3365155e1 | 98 | return 1; |
benparkes | 1:f4e3365155e1 | 99 | } |
benparkes | 1:f4e3365155e1 | 100 | |
benparkes | 1:f4e3365155e1 | 101 | |
benparkes | 1:f4e3365155e1 | 102 | |
benparkes | 1:f4e3365155e1 | 103 |