鄭 旭航 / hallsensor_software_decoder

fdsaf

hallsensor_software_decoder.h@3:a969ae9954a8, 2019-03-27 (annotated)

Committer:
bobolee1239
Date:
Wed Mar 27 03:08:46 2019 +0000
Revision:
3:a969ae9954a8
Parent:
2:b041de36c002
Child:
4:8ce9fbe46da2
add ros lib

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 1:878564121574 53 wheelState1.state = (wheelState1.hallA << 1) + (wheelState1.hallA ^ wheelState1.hallB) + 1;
bobolee1239 0:0532a6e10a23 54
bobolee1239 0:0532a6e10a23 55
bobolee1239 3:a969ae9954a8 56 if(wheelState1.state == 1) {
bobolee1239 3:a969ae9954a8 57 if(wheelState1.prestate == 4) {
bobolee1239 1:878564121574 58 wheelState1.direction = true;
bobolee1239 3:a969ae9954a8 59 } else if(wheelState1.prestate != 1) {
bobolee1239 1:878564121574 60 wheelState1.direction = false;
bobolee1239 0:0532a6e10a23 61 }
bobolee1239 0:0532a6e10a23 62 }
bobolee1239 1:878564121574 63 else if(wheelState1.state == 4){
bobolee1239 1:878564121574 64 if(wheelState1.prestate == 1){
bobolee1239 1:878564121574 65 wheelState1.direction = false;
bobolee1239 1:878564121574 66 } else if(wheelState1.prestate != 4) {
bobolee1239 1:878564121574 67 wheelState1.direction = true;
bobolee1239 0:0532a6e10a23 68 }
bobolee1239 0:0532a6e10a23 69 }
bobolee1239 0:0532a6e10a23 70 else {
bobolee1239 3:a969ae9954a8 71 if(wheelState1.state > wheelState1.prestate) {
bobolee1239 1:878564121574 72 wheelState1.direction = true;
bobolee1239 3:a969ae9954a8 73 } else if(wheelState1.state < wheelState1.prestate) {
bobolee1239 1:878564121574 74 wheelState1.direction = false;
bobolee1239 0:0532a6e10a23 75 }
bobolee1239 0:0532a6e10a23 76 }
bobolee1239 0:0532a6e10a23 77
bobolee1239 0:0532a6e10a23 78 /* do nothing if state ain't change */
bobolee1239 3:a969ae9954a8 79 if(wheelState1.state != wheelState1.prestate) {
bobolee1239 1:878564121574 80 if(wheelState1.direction) {
bobolee1239 1:878564121574 81 ++wheelState1.numStateChange;
bobolee1239 1:878564121574 82 } else {
bobolee1239 1:878564121574 83 --wheelState1.numStateChange;
bobolee1239 1:878564121574 84 }
bobolee1239 0:0532a6e10a23 85 }
bobolee1239 0:0532a6e10a23 86 /* update previous state */
bobolee1239 1:878564121574 87 wheelState1.prestate = wheelState1.state;
bobolee1239 0:0532a6e10a23 88
bobolee1239 0:0532a6e10a23 89 /* MOTOR2 */
bobolee1239 1:878564121574 90 wheelState2.hallA = HallA_2.read();
bobolee1239 1:878564121574 91 wheelState2.hallB = HallB_2.read();
bobolee1239 0:0532a6e10a23 92
bobolee1239 0:0532a6e10a23 93 /* state determination */
bobolee1239 1:878564121574 94 wheelState2.state = (wheelState2.hallA << 1) + (wheelState2.hallA ^ wheelState2.hallB) + 1;
bobolee1239 0:0532a6e10a23 95
bobolee1239 0:0532a6e10a23 96
bobolee1239 1:878564121574 97 if(wheelState2.state == 1){
bobolee1239 1:878564121574 98 if(wheelState2.prestate == 4){
bobolee1239 1:878564121574 99 wheelState2.direction = true;
bobolee1239 1:878564121574 100 } else if(wheelState2.prestate != 1){
bobolee1239 1:878564121574 101 wheelState2.direction = false;
bobolee1239 0:0532a6e10a23 102 }
bobolee1239 0:0532a6e10a23 103 }
bobolee1239 1:878564121574 104 else if(wheelState2.state == 4){
bobolee1239 1:878564121574 105 if(wheelState2.prestate == 1){
bobolee1239 1:878564121574 106 wheelState2.direction = false;
bobolee1239 1:878564121574 107 } else if(wheelState2.prestate != 4){
bobolee1239 1:878564121574 108 wheelState2.direction = true;
bobolee1239 0:0532a6e10a23 109 }
bobolee1239 0:0532a6e10a23 110 }
bobolee1239 0:0532a6e10a23 111 else{
bobolee1239 1:878564121574 112 if(wheelState2.state > wheelState2.prestate){
bobolee1239 1:878564121574 113 wheelState2.direction = true;
bobolee1239 1:878564121574 114 } else if(wheelState2.state < wheelState2.prestate){
bobolee1239 1:878564121574 115 wheelState2.direction = false;
bobolee1239 0:0532a6e10a23 116 }
bobolee1239 1:878564121574 117 }
bobolee1239 1:878564121574 118
bobolee1239 0:0532a6e10a23 119 /* do nothing if state ain't change */
bobolee1239 1:878564121574 120 if(wheelState2.state != wheelState2.prestate){
bobolee1239 1:878564121574 121 if(wheelState2.direction) {
bobolee1239 1:878564121574 122 ++wheelState2.numStateChange;
bobolee1239 1:878564121574 123 } else {
bobolee1239 1:878564121574 124 --wheelState2.numStateChange;
bobolee1239 1:878564121574 125 }
bobolee1239 0:0532a6e10a23 126 }
bobolee1239 0:0532a6e10a23 127 /* update previous state */
bobolee1239 1:878564121574 128 wheelState2.prestate = wheelState2.state;
bobolee1239 0:0532a6e10a23 129 }
bobolee1239 0:0532a6e10a23 130
bobolee1239 0:0532a6e10a23 131
bobolee1239 2:b041de36c002 132 #endif // _HALLSENSOR_SOFTWARE_DECODER_H