encoder and tachometer class for EW3XX single board computer
EW305sbc.cpp@1:b33d4964669b, 2019-08-16 (annotated)
- Committer:
- lddevrie
- Date:
- Fri Aug 16 19:05:24 2019 +0000
- Revision:
- 1:b33d4964669b
- Parent:
- 0:1d5bf13d7dbb
- Child:
- 2:6cf351721d86
fixed sign of speed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lddevrie | 0:1d5bf13d7dbb | 1 | //#include "mbed.h" |
lddevrie | 0:1d5bf13d7dbb | 2 | #include "EW305sbc.h" |
lddevrie | 0:1d5bf13d7dbb | 3 | |
lddevrie | 0:1d5bf13d7dbb | 4 | |
lddevrie | 0:1d5bf13d7dbb | 5 | EW305sbc::EW305sbc(int ch, int pulsesPerRev) |
lddevrie | 0:1d5bf13d7dbb | 6 | : ch_(ch), pulsesPerRev_(pulsesPerRev), spi(p5, p6, p7), ls7166_cs1(p19,1), ls7166_cs2(p20,1) |
lddevrie | 0:1d5bf13d7dbb | 7 | { |
lddevrie | 0:1d5bf13d7dbb | 8 | init(); |
lddevrie | 0:1d5bf13d7dbb | 9 | } |
lddevrie | 0:1d5bf13d7dbb | 10 | |
lddevrie | 0:1d5bf13d7dbb | 11 | void EW305sbc::init() |
lddevrie | 0:1d5bf13d7dbb | 12 | { |
lddevrie | 0:1d5bf13d7dbb | 13 | dt_ = 0.00625; // 125 Hz |
lddevrie | 0:1d5bf13d7dbb | 14 | speed_ = 0.0; // initial distance |
lddevrie | 0:1d5bf13d7dbb | 15 | countp_ = 0; |
lddevrie | 0:1d5bf13d7dbb | 16 | count_ = 0; |
lddevrie | 0:1d5bf13d7dbb | 17 | /** configure the rising edge to start the timer */ |
lddevrie | 0:1d5bf13d7dbb | 18 | updater.attach(callback(this, &EW305sbc::recalc), 0.00625); // 125 Hz, syntax from https://os.mbed.com/forum/mbed/topic/1964/?page=1 |
lddevrie | 0:1d5bf13d7dbb | 19 | wait(.2); //delay at beginning for voltage settle purposes |
lddevrie | 0:1d5bf13d7dbb | 20 | |
lddevrie | 0:1d5bf13d7dbb | 21 | // initialize channel 1 encoder by |
lddevrie | 0:1d5bf13d7dbb | 22 | LS7366_reset_counter(ch_); |
lddevrie | 0:1d5bf13d7dbb | 23 | LS7366_quad_mode_x4(ch_); |
lddevrie | 0:1d5bf13d7dbb | 24 | LS7366_write_DTR(ch_,0); |
lddevrie | 0:1d5bf13d7dbb | 25 | } |
lddevrie | 0:1d5bf13d7dbb | 26 | |
lddevrie | 0:1d5bf13d7dbb | 27 | |
lddevrie | 0:1d5bf13d7dbb | 28 | void EW305sbc::recalc() |
lddevrie | 0:1d5bf13d7dbb | 29 | { |
lddevrie | 0:1d5bf13d7dbb | 30 | // Read encoder |
lddevrie | 1:b33d4964669b | 31 | count_ = LS7366_read_counter(ch_); // input is the encoder channel |
lddevrie | 0:1d5bf13d7dbb | 32 | |
lddevrie | 0:1d5bf13d7dbb | 33 | // Estimate speed (counts/sec) |
lddevrie | 1:b33d4964669b | 34 | speed_ = -(count_-countp_)/0.00625*2.0*3.14159/(4.0*pulsesPerRev_); |
lddevrie | 0:1d5bf13d7dbb | 35 | |
lddevrie | 0:1d5bf13d7dbb | 36 | // Age variable |
lddevrie | 0:1d5bf13d7dbb | 37 | countp_ = count_; |
lddevrie | 0:1d5bf13d7dbb | 38 | } |
lddevrie | 0:1d5bf13d7dbb | 39 | |
lddevrie | 0:1d5bf13d7dbb | 40 | |
lddevrie | 0:1d5bf13d7dbb | 41 | int EW305sbc::getCount(void) |
lddevrie | 0:1d5bf13d7dbb | 42 | { |
lddevrie | 0:1d5bf13d7dbb | 43 | return count_; |
lddevrie | 0:1d5bf13d7dbb | 44 | } |
lddevrie | 0:1d5bf13d7dbb | 45 | |
lddevrie | 0:1d5bf13d7dbb | 46 | float EW305sbc::getSpeed(void) |
lddevrie | 0:1d5bf13d7dbb | 47 | { |
lddevrie | 0:1d5bf13d7dbb | 48 | return speed_; |
lddevrie | 0:1d5bf13d7dbb | 49 | } |
lddevrie | 0:1d5bf13d7dbb | 50 | |
lddevrie | 0:1d5bf13d7dbb | 51 | |
lddevrie | 0:1d5bf13d7dbb | 52 | |
lddevrie | 0:1d5bf13d7dbb | 53 | |
lddevrie | 0:1d5bf13d7dbb | 54 | |
lddevrie | 0:1d5bf13d7dbb | 55 | |
lddevrie | 0:1d5bf13d7dbb | 56 | //---- Function Listing ------------------------------- |
lddevrie | 0:1d5bf13d7dbb | 57 | |
lddevrie | 0:1d5bf13d7dbb | 58 | //----- LS7366 Encoder/Counter Routines -------------------- |
lddevrie | 0:1d5bf13d7dbb | 59 | void EW305sbc::LS7366_cmd(int inst, int reg) |
lddevrie | 0:1d5bf13d7dbb | 60 | { |
lddevrie | 0:1d5bf13d7dbb | 61 | char cmd; |
lddevrie | 0:1d5bf13d7dbb | 62 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 63 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 64 | cmd = (inst << 6) | (reg << 3); |
lddevrie | 0:1d5bf13d7dbb | 65 | // printf("\r\ncmd=0X%2X", cmd); |
lddevrie | 0:1d5bf13d7dbb | 66 | spi.write(cmd); |
lddevrie | 0:1d5bf13d7dbb | 67 | } |
lddevrie | 0:1d5bf13d7dbb | 68 | |
lddevrie | 0:1d5bf13d7dbb | 69 | long EW305sbc::LS7366_read_counter(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 70 | { |
lddevrie | 0:1d5bf13d7dbb | 71 | union bytes { |
lddevrie | 0:1d5bf13d7dbb | 72 | char byte_enc[4]; |
lddevrie | 0:1d5bf13d7dbb | 73 | long long_enc; |
lddevrie | 0:1d5bf13d7dbb | 74 | } counter; |
lddevrie | 0:1d5bf13d7dbb | 75 | |
lddevrie | 0:1d5bf13d7dbb | 76 | counter.long_enc = 0; |
lddevrie | 0:1d5bf13d7dbb | 77 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 78 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 79 | |
lddevrie | 0:1d5bf13d7dbb | 80 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 81 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 82 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 83 | LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR |
lddevrie | 0:1d5bf13d7dbb | 84 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 85 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 86 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 87 | } else { |
lddevrie | 0:1d5bf13d7dbb | 88 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 89 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 90 | LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR |
lddevrie | 0:1d5bf13d7dbb | 91 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 92 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 93 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 94 | } |
lddevrie | 0:1d5bf13d7dbb | 95 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 96 | LS7366_cmd(RD,CNTR); //cmd = 0x60, READ from CNTR |
lddevrie | 0:1d5bf13d7dbb | 97 | counter.byte_enc[3] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 98 | counter.byte_enc[2] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 99 | counter.byte_enc[1] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 100 | counter.byte_enc[0] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 101 | |
lddevrie | 0:1d5bf13d7dbb | 102 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 103 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 104 | } else { |
lddevrie | 0:1d5bf13d7dbb | 105 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 106 | } |
lddevrie | 0:1d5bf13d7dbb | 107 | |
lddevrie | 0:1d5bf13d7dbb | 108 | return counter.long_enc; //return count |
lddevrie | 0:1d5bf13d7dbb | 109 | } |
lddevrie | 0:1d5bf13d7dbb | 110 | |
lddevrie | 0:1d5bf13d7dbb | 111 | void EW305sbc::LS7366_quad_mode_x4(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 112 | { |
lddevrie | 0:1d5bf13d7dbb | 113 | |
lddevrie | 0:1d5bf13d7dbb | 114 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 115 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 116 | |
lddevrie | 0:1d5bf13d7dbb | 117 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 118 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 119 | } else { |
lddevrie | 0:1d5bf13d7dbb | 120 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 121 | } |
lddevrie | 0:1d5bf13d7dbb | 122 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 123 | LS7366_cmd(WR,MDR0);// Write to the MDR0 register |
lddevrie | 0:1d5bf13d7dbb | 124 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 125 | spi.write(0x03); // X4 quadrature count mode |
lddevrie | 0:1d5bf13d7dbb | 126 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 127 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 128 | } else { |
lddevrie | 0:1d5bf13d7dbb | 129 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 130 | } |
lddevrie | 0:1d5bf13d7dbb | 131 | } |
lddevrie | 0:1d5bf13d7dbb | 132 | |
lddevrie | 0:1d5bf13d7dbb | 133 | void EW305sbc::LS7366_reset_counter(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 134 | { |
lddevrie | 0:1d5bf13d7dbb | 135 | spi.format(8, 0); // set up SPI for 8 data bits, mode 0 |
lddevrie | 0:1d5bf13d7dbb | 136 | spi.frequency(2000000); // 2MHz SPI clock |
lddevrie | 0:1d5bf13d7dbb | 137 | |
lddevrie | 0:1d5bf13d7dbb | 138 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 139 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 140 | } else { |
lddevrie | 0:1d5bf13d7dbb | 141 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 142 | } |
lddevrie | 0:1d5bf13d7dbb | 143 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 144 | LS7366_cmd(CLR,CNTR); // Clear the counter register |
lddevrie | 0:1d5bf13d7dbb | 145 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 146 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 147 | } else { |
lddevrie | 0:1d5bf13d7dbb | 148 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 149 | } |
lddevrie | 0:1d5bf13d7dbb | 150 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 151 | |
lddevrie | 0:1d5bf13d7dbb | 152 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 153 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 154 | } else { |
lddevrie | 0:1d5bf13d7dbb | 155 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 156 | } |
lddevrie | 0:1d5bf13d7dbb | 157 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 158 | LS7366_cmd(LOAD,CNTR); // load counter reg |
lddevrie | 0:1d5bf13d7dbb | 159 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 160 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 161 | } else { |
lddevrie | 0:1d5bf13d7dbb | 162 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 163 | } |
lddevrie | 0:1d5bf13d7dbb | 164 | } |
lddevrie | 0:1d5bf13d7dbb | 165 | |
lddevrie | 0:1d5bf13d7dbb | 166 | void EW305sbc::LS7366_write_DTR(int chan_num, long enc_value) |
lddevrie | 0:1d5bf13d7dbb | 167 | { |
lddevrie | 0:1d5bf13d7dbb | 168 | union bytes { // Union to speed up byte writes |
lddevrie | 0:1d5bf13d7dbb | 169 | char byte_enc[4]; |
lddevrie | 0:1d5bf13d7dbb | 170 | long long_enc; |
lddevrie | 0:1d5bf13d7dbb | 171 | } counter; |
lddevrie | 0:1d5bf13d7dbb | 172 | |
lddevrie | 0:1d5bf13d7dbb | 173 | spi.format(8, 0); // set up SPI for 8 data bits, mode 0 |
lddevrie | 0:1d5bf13d7dbb | 174 | spi.frequency(2000000); // 2MHz SPI clock |
lddevrie | 0:1d5bf13d7dbb | 175 | |
lddevrie | 0:1d5bf13d7dbb | 176 | counter.long_enc = enc_value; // pass enc_value to Union |
lddevrie | 0:1d5bf13d7dbb | 177 | |
lddevrie | 0:1d5bf13d7dbb | 178 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 179 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 180 | } else { |
lddevrie | 0:1d5bf13d7dbb | 181 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 182 | } |
lddevrie | 0:1d5bf13d7dbb | 183 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 184 | LS7366_cmd(WR,DTR); // Write to the Data Transfer Register |
lddevrie | 0:1d5bf13d7dbb | 185 | spi.write(counter.byte_enc[3]); // Write the 32-bit encoder value |
lddevrie | 0:1d5bf13d7dbb | 186 | spi.write(counter.byte_enc[2]); |
lddevrie | 0:1d5bf13d7dbb | 187 | spi.write(counter.byte_enc[1]); |
lddevrie | 0:1d5bf13d7dbb | 188 | spi.write(counter.byte_enc[0]); |
lddevrie | 0:1d5bf13d7dbb | 189 | if(chan_num!=2) { // de-activate the chip select |
lddevrie | 0:1d5bf13d7dbb | 190 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 191 | } else { |
lddevrie | 0:1d5bf13d7dbb | 192 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 193 | } |
lddevrie | 0:1d5bf13d7dbb | 194 | |
lddevrie | 0:1d5bf13d7dbb | 195 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 196 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 197 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 198 | } else { |
lddevrie | 0:1d5bf13d7dbb | 199 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 200 | } |
lddevrie | 0:1d5bf13d7dbb | 201 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 202 | LS7366_cmd(LOAD,CNTR); // load command to the counter register from DTR |
lddevrie | 0:1d5bf13d7dbb | 203 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 204 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 205 | } else { |
lddevrie | 0:1d5bf13d7dbb | 206 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 207 | } |
lddevrie | 0:1d5bf13d7dbb | 208 | } |