Brian Lee / hallsensor_software_decoder

hall sensor decoder

Dependents:   mobile_robot_lab3_rosserial mobile_robot_lab3_rosserial mobile-robot-vehicle-control

hallsensor_software_decoder.h@0:0532a6e10a23, 2019-03-13 (annotated)

Committer:
bobolee1239
Date:
Wed Mar 13 06:22:03 2019 +0000
Revision:
0:0532a6e10a23
Child:
1:878564121574
revised original code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bobolee1239 0:0532a6e10a23 1 /*
bobolee1239 0:0532a6e10a23 2 software decoder is for motor2
bobolee1239 0:0532a6e10a23 3 "speed_count_2" can save the decoded data after one run the function "init_CH()"
bobolee1239 0:0532a6e10a23 4 */
bobolee1239 0:0532a6e10a23 5
bobolee1239 0:0532a6e10a23 6 #ifndef _HALLSENSOR_SOFTWARE_DECODER_H
bobolee1239 0:0532a6e10a23 7 #define _HALLSENSOR_SOFTWARE_DECODER_H
bobolee1239 0:0532a6e10a23 8
bobolee1239 0:0532a6e10a23 9
bobolee1239 0:0532a6e10a23 10 #include "mbed.h"
bobolee1239 0:0532a6e10a23 11
bobolee1239 0:0532a6e10a23 12 InterruptIn HallA_1(A1);
bobolee1239 0:0532a6e10a23 13 InterruptIn HallB_1(A2);
bobolee1239 0:0532a6e10a23 14 InterruptIn HallA_2(D13);
bobolee1239 0:0532a6e10a23 15 InterruptIn HallB_2(D12);
bobolee1239 0:0532a6e10a23 16
bobolee1239 0:0532a6e10a23 17 void init_CN();
bobolee1239 0:0532a6e10a23 18 void CN_ITR();
bobolee1239 0:0532a6e10a23 19
bobolee1239 0:0532a6e10a23 20 /*********************************************
bobolee1239 0:0532a6e10a23 21 * for_back = 1 -> forward
bobolee1239 0:0532a6e10a23 22 * for_back = 0 -> backward
bobolee1239 0:0532a6e10a23 23 *********************************************/
bobolee1239 0:0532a6e10a23 24 int for_back_1 = 0;
bobolee1239 0:0532a6e10a23 25 int for_back_2 = 0;
bobolee1239 0:0532a6e10a23 26
bobolee1239 0:0532a6e10a23 27 int HallA_1_state = 0;
bobolee1239 0:0532a6e10a23 28 int HallB_1_state = 0;
bobolee1239 0:0532a6e10a23 29 int state_1 = 0;
bobolee1239 0:0532a6e10a23 30 int state_1_old = 0;
bobolee1239 0:0532a6e10a23 31
bobolee1239 0:0532a6e10a23 32 int HallA_2_state = 0;
bobolee1239 0:0532a6e10a23 33 int HallB_2_state = 0;
bobolee1239 0:0532a6e10a23 34 int state_2 = 0;
bobolee1239 0:0532a6e10a23 35 int state_2_old = 0;
bobolee1239 0:0532a6e10a23 36
bobolee1239 0:0532a6e10a23 37 int speed_count_1;
bobolee1239 0:0532a6e10a23 38 int speed_count_2;
bobolee1239 0:0532a6e10a23 39
bobolee1239 0:0532a6e10a23 40 void init_CN(){
bobolee1239 0:0532a6e10a23 41 HallA_1.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 42 HallA_1.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 43 HallB_1.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 44 HallB_1.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 45 HallA_2.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 46 HallA_2.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 47 HallB_2.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 48 HallB_2.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 49
bobolee1239 0:0532a6e10a23 50 }
bobolee1239 0:0532a6e10a23 51
bobolee1239 0:0532a6e10a23 52 void CN_ITR() {
bobolee1239 0:0532a6e10a23 53 /* MOTOR1 */
bobolee1239 0:0532a6e10a23 54 HallA_1_state = HallA_1.read();
bobolee1239 0:0532a6e10a23 55 HallB_1_state = HallB_1.read();
bobolee1239 0:0532a6e10a23 56
bobolee1239 0:0532a6e10a23 57 /* state determination */
bobolee1239 0:0532a6e10a23 58 state_1 = (HallA_1_state << 1) + (((~HallA_1_state)&&HallB_1_state) || (HallA_1_state && (~HallB_1_state))) + 1;
bobolee1239 0:0532a6e10a23 59
bobolee1239 0:0532a6e10a23 60
bobolee1239 0:0532a6e10a23 61 if(state_1 == 1){
bobolee1239 0:0532a6e10a23 62 if(state_1_old == 4){
bobolee1239 0:0532a6e10a23 63 for_back_1 = 1;
bobolee1239 0:0532a6e10a23 64 } else if(state_1_old != 1){
bobolee1239 0:0532a6e10a23 65 for_back_1 = 0;
bobolee1239 0:0532a6e10a23 66 }
bobolee1239 0:0532a6e10a23 67 }
bobolee1239 0:0532a6e10a23 68 else if(state_1 == 4){
bobolee1239 0:0532a6e10a23 69 if(state_1_old == 1){
bobolee1239 0:0532a6e10a23 70 for_back_1 = 0;
bobolee1239 0:0532a6e10a23 71 } else if(state_1_old != 4) {
bobolee1239 0:0532a6e10a23 72 for_back_1 = 1;
bobolee1239 0:0532a6e10a23 73 }
bobolee1239 0:0532a6e10a23 74 }
bobolee1239 0:0532a6e10a23 75 else {
bobolee1239 0:0532a6e10a23 76 if(state_1 > state_1_old){
bobolee1239 0:0532a6e10a23 77 for_back_1 = 1;
bobolee1239 0:0532a6e10a23 78 }else if(state_1 < state_1_old){
bobolee1239 0:0532a6e10a23 79 for_back_1 = 0;
bobolee1239 0:0532a6e10a23 80 }
bobolee1239 0:0532a6e10a23 81 }
bobolee1239 0:0532a6e10a23 82
bobolee1239 0:0532a6e10a23 83 /* do nothing if state ain't change */
bobolee1239 0:0532a6e10a23 84 if(state_1_old != state_1){
bobolee1239 0:0532a6e10a23 85 (for_back_1)? ++speed_count_1 : --speed_count_1;
bobolee1239 0:0532a6e10a23 86 }
bobolee1239 0:0532a6e10a23 87 /* update previous state */
bobolee1239 0:0532a6e10a23 88 state_1_old = state_1;
bobolee1239 0:0532a6e10a23 89
bobolee1239 0:0532a6e10a23 90
bobolee1239 0:0532a6e10a23 91 /* MOTOR2 */
bobolee1239 0:0532a6e10a23 92 HallA_2_state = HallA_2.read();
bobolee1239 0:0532a6e10a23 93 HallB_2_state = HallB_2.read();
bobolee1239 0:0532a6e10a23 94
bobolee1239 0:0532a6e10a23 95 /* state determination */
bobolee1239 0:0532a6e10a23 96 state_2 = (HallA_2_state << 1) + (((~HallA_2_state)&&HallB_2_state) || (HallA_2_state && (~HallB_2_state))) + 1;
bobolee1239 0:0532a6e10a23 97
bobolee1239 0:0532a6e10a23 98
bobolee1239 0:0532a6e10a23 99 if(state_2 == 1){
bobolee1239 0:0532a6e10a23 100 if(state_2_old == 4){
bobolee1239 0:0532a6e10a23 101 for_back_2 = 1;
bobolee1239 0:0532a6e10a23 102 } else if(state_2_old != 1){
bobolee1239 0:0532a6e10a23 103 for_back_2 = 0;
bobolee1239 0:0532a6e10a23 104 }
bobolee1239 0:0532a6e10a23 105 }
bobolee1239 0:0532a6e10a23 106 else if(state_2 == 4){
bobolee1239 0:0532a6e10a23 107 if(state_2_old == 1){
bobolee1239 0:0532a6e10a23 108 for_back_2 = 0;
bobolee1239 0:0532a6e10a23 109 } else if(state_2_old != 4){
bobolee1239 0:0532a6e10a23 110 for_back_2 = 1;
bobolee1239 0:0532a6e10a23 111 }
bobolee1239 0:0532a6e10a23 112 }
bobolee1239 0:0532a6e10a23 113 else{
bobolee1239 0:0532a6e10a23 114 if(state_2 > state_2_old){
bobolee1239 0:0532a6e10a23 115 for_back_2 = 1;
bobolee1239 0:0532a6e10a23 116 } else if(state_2 < state_2_old){
bobolee1239 0:0532a6e10a23 117 for_back_2 = 0;
bobolee1239 0:0532a6e10a23 118 }
bobolee1239 0:0532a6e10a23 119 }
bobolee1239 0:0532a6e10a23 120 /* do nothing if state ain't change */
bobolee1239 0:0532a6e10a23 121 if(state_2_old != state_2){
bobolee1239 0:0532a6e10a23 122 (for_back_2)? ++speed_count_2 : --speed_count_2;
bobolee1239 0:0532a6e10a23 123 }
bobolee1239 0:0532a6e10a23 124 /* update previous state */
bobolee1239 0:0532a6e10a23 125 state_2_old = state_2;
bobolee1239 0:0532a6e10a23 126 }
bobolee1239 0:0532a6e10a23 127
bobolee1239 0:0532a6e10a23 128
bobolee1239 0:0532a6e10a23 129 #endif