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@5:b6d81c106fd1, 2019-03-29 (annotated)

Committer:
bobolee1239
Date:
Fri Mar 29 05:59:43 2019 +0000
Revision:
5:b6d81c106fd1
Parent:
4:553de07891f2 
revised decoder (correct -> wrong) --- Brian

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 1:878564121574 20 typedef volatile struct WheelState {
bobolee1239 1:878564121574 21 volatile int hallA;
bobolee1239 1:878564121574 22 volatile int hallB;
bobolee1239 1:878564121574 23 volatile unsigned short state;
bobolee1239 1:878564121574 24 volatile unsigned short prestate;
bobolee1239 1:878564121574 25 volatile int numStateChange;
bobolee1239 1:878564121574 26 volatile bool direction; // true: forward, false: backward
bobolee1239 1:878564121574 27 } WheelState_t;
bobolee1239 0:0532a6e10a23 28
bobolee1239 1:878564121574 29 WheelState_t wheelState1, wheelState2;
bobolee1239 0:0532a6e10a23 30
bobolee1239 3:a969ae9954a8 31 void init_CN() {
bobolee1239 0:0532a6e10a23 32 HallA_1.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 33 HallA_1.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 34 HallB_1.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 35 HallB_1.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 36 HallA_2.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 37 HallA_2.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 38 HallB_2.rise(&CN_ITR);
bobolee1239 0:0532a6e10a23 39 HallB_2.fall(&CN_ITR);
bobolee1239 0:0532a6e10a23 40
bobolee1239 0:0532a6e10a23 41 }
bobolee1239 0:0532a6e10a23 42
bobolee1239 0:0532a6e10a23 43 void CN_ITR() {
bobolee1239 0:0532a6e10a23 44 /* MOTOR1 */
bobolee1239 1:878564121574 45 wheelState1.hallA = HallA_1.read();
bobolee1239 1:878564121574 46 wheelState1.hallB = HallB_1.read();
bobolee1239 0:0532a6e10a23 47
bobolee1239 1:878564121574 48 /********************************
bobolee1239 1:878564121574 49 ** State Estimation
bobolee1239 1:878564121574 50 ** 1. hallA = 0, 1
bobolee1239 1:878564121574 51 ** 2. hallB = 0, 1
bobolee1239 1:878564121574 52 ********************************/
bobolee1239 5:b6d81c106fd1 53 // wheelState1.state = (wheelState1.hallA << 1) + ((wheelState1.hallA ^ wheelState1.hallB) & 0x0001) + 1;
bobolee1239 5:b6d81c106fd1 54 wheelState1.state = (wheelState1.hallA << 1)
bobolee1239 5:b6d81c106fd1 55 + (((~wheelState1.hallA) && wheelState1.hallB) || (wheelState1.hallA && (~wheelState1.hallB)))
bobolee1239 5:b6d81c106fd1 56 + 1;
bobolee1239 0:0532a6e10a23 57
bobolee1239 0:0532a6e10a23 58
bobolee1239 3:a969ae9954a8 59 if(wheelState1.state == 1) {
bobolee1239 3:a969ae9954a8 60 if(wheelState1.prestate == 4) {
bobolee1239 1:878564121574 61 wheelState1.direction = true;
bobolee1239 3:a969ae9954a8 62 } else if(wheelState1.prestate != 1) {
bobolee1239 1:878564121574 63 wheelState1.direction = false;
bobolee1239 0:0532a6e10a23 64 }
bobolee1239 0:0532a6e10a23 65 }
bobolee1239 1:878564121574 66 else if(wheelState1.state == 4){
bobolee1239 1:878564121574 67 if(wheelState1.prestate == 1){
bobolee1239 1:878564121574 68 wheelState1.direction = false;
bobolee1239 1:878564121574 69 } else if(wheelState1.prestate != 4) {
bobolee1239 1:878564121574 70 wheelState1.direction = true;
bobolee1239 0:0532a6e10a23 71 }
bobolee1239 0:0532a6e10a23 72 }
bobolee1239 0:0532a6e10a23 73 else {
bobolee1239 3:a969ae9954a8 74 if(wheelState1.state > wheelState1.prestate) {
bobolee1239 1:878564121574 75 wheelState1.direction = true;
bobolee1239 3:a969ae9954a8 76 } else if(wheelState1.state < wheelState1.prestate) {
bobolee1239 1:878564121574 77 wheelState1.direction = false;
bobolee1239 0:0532a6e10a23 78 }
bobolee1239 0:0532a6e10a23 79 }
bobolee1239 0:0532a6e10a23 80
bobolee1239 0:0532a6e10a23 81 /* do nothing if state ain't change */
bobolee1239 3:a969ae9954a8 82 if(wheelState1.state != wheelState1.prestate) {
bobolee1239 1:878564121574 83 if(wheelState1.direction) {
bobolee1239 1:878564121574 84 ++wheelState1.numStateChange;
bobolee1239 1:878564121574 85 } else {
bobolee1239 1:878564121574 86 --wheelState1.numStateChange;
bobolee1239 1:878564121574 87 }
bobolee1239 0:0532a6e10a23 88 }
bobolee1239 0:0532a6e10a23 89 /* update previous state */
bobolee1239 1:878564121574 90 wheelState1.prestate = wheelState1.state;
bobolee1239 0:0532a6e10a23 91
bobolee1239 0:0532a6e10a23 92 /* MOTOR2 */
bobolee1239 1:878564121574 93 wheelState2.hallA = HallA_2.read();
bobolee1239 1:878564121574 94 wheelState2.hallB = HallB_2.read();
bobolee1239 0:0532a6e10a23 95
bobolee1239 0:0532a6e10a23 96 /* state determination */
bobolee1239 5:b6d81c106fd1 97 // wheelState2.state = (wheelState2.hallA << 1) + ((wheelState2.hallA ^ wheelState2.hallB)&0x0001) + 1;
bobolee1239 5:b6d81c106fd1 98 wheelState2.state = (wheelState2.hallA << 1)
bobolee1239 5:b6d81c106fd1 99 + (((~wheelState2.hallA) && wheelState2.hallB) || (wheelState2.hallA && (~wheelState2.hallB)))
bobolee1239 5:b6d81c106fd1 100 + 1;
bobolee1239 5:b6d81c106fd1 101
bobolee1239 0:0532a6e10a23 102
bobolee1239 1:878564121574 103 if(wheelState2.state == 1){
bobolee1239 1:878564121574 104 if(wheelState2.prestate == 4){
bobolee1239 1:878564121574 105 wheelState2.direction = true;
bobolee1239 1:878564121574 106 } else if(wheelState2.prestate != 1){
bobolee1239 1:878564121574 107 wheelState2.direction = false;
bobolee1239 0:0532a6e10a23 108 }
bobolee1239 0:0532a6e10a23 109 }
bobolee1239 1:878564121574 110 else if(wheelState2.state == 4){
bobolee1239 1:878564121574 111 if(wheelState2.prestate == 1){
bobolee1239 1:878564121574 112 wheelState2.direction = false;
bobolee1239 1:878564121574 113 } else if(wheelState2.prestate != 4){
bobolee1239 1:878564121574 114 wheelState2.direction = true;
bobolee1239 0:0532a6e10a23 115 }
bobolee1239 0:0532a6e10a23 116 }
bobolee1239 0:0532a6e10a23 117 else{
bobolee1239 1:878564121574 118 if(wheelState2.state > wheelState2.prestate){
bobolee1239 1:878564121574 119 wheelState2.direction = true;
bobolee1239 1:878564121574 120 } else if(wheelState2.state < wheelState2.prestate){
bobolee1239 1:878564121574 121 wheelState2.direction = false;
bobolee1239 0:0532a6e10a23 122 }
bobolee1239 1:878564121574 123 }
bobolee1239 1:878564121574 124
bobolee1239 0:0532a6e10a23 125 /* do nothing if state ain't change */
bobolee1239 1:878564121574 126 if(wheelState2.state != wheelState2.prestate){
bobolee1239 1:878564121574 127 if(wheelState2.direction) {
bobolee1239 1:878564121574 128 ++wheelState2.numStateChange;
bobolee1239 1:878564121574 129 } else {
bobolee1239 1:878564121574 130 --wheelState2.numStateChange;
bobolee1239 1:878564121574 131 }
bobolee1239 0:0532a6e10a23 132 }
bobolee1239 0:0532a6e10a23 133 /* update previous state */
bobolee1239 1:878564121574 134 wheelState2.prestate = wheelState2.state;
bobolee1239 0:0532a6e10a23 135 }
bobolee1239 0:0532a6e10a23 136
bobolee1239 0:0532a6e10a23 137
bobolee1239 2:b041de36c002 138 #endif // _HALLSENSOR_SOFTWARE_DECODER_H