Coursework Buggy / Mbed 2 deprecated 161015

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?

UserRevisionLine numberNew 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