Levi DeVries / EW305sbc

encoder and tachometer class for EW3XX single board computer

EW305sbc.cpp@2:6cf351721d86, 2019-09-16 (annotated)

Committer:
lddevrie
Date:
Mon Sep 16 23:34:45 2019 +0000
Revision:
2:6cf351721d86
Parent:
1:b33d4964669b
Child:
4:19ee7eb16605
Added getAngle method.

Who changed what in which revision?

UserRevisionLine numberNew 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 2:6cf351721d86 17 angle_ = 0.0;
lddevrie 0:1d5bf13d7dbb 18 /** configure the rising edge to start the timer */
lddevrie 0:1d5bf13d7dbb 19 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 20 wait(.2); //delay at beginning for voltage settle purposes
lddevrie 0:1d5bf13d7dbb 21
lddevrie 2:6cf351721d86 22 // initialize channel ch_ encoder by
lddevrie 0:1d5bf13d7dbb 23 LS7366_reset_counter(ch_);
lddevrie 0:1d5bf13d7dbb 24 LS7366_quad_mode_x4(ch_);
lddevrie 0:1d5bf13d7dbb 25 LS7366_write_DTR(ch_,0);
lddevrie 0:1d5bf13d7dbb 26 }
lddevrie 0:1d5bf13d7dbb 27
lddevrie 0:1d5bf13d7dbb 28
lddevrie 0:1d5bf13d7dbb 29 void EW305sbc::recalc()
lddevrie 0:1d5bf13d7dbb 30 {
lddevrie 0:1d5bf13d7dbb 31 // Read encoder
lddevrie 1:b33d4964669b 32 count_ = LS7366_read_counter(ch_); // input is the encoder channel
lddevrie 2:6cf351721d86 33
lddevrie 2:6cf351721d86 34 // convert count to angle
lddevrie 2:6cf351721d86 35 angle_ = count_*2.0*3.14159/(4.0*pulsesPerRev_);
lddevrie 2:6cf351721d86 36
lddevrie 0:1d5bf13d7dbb 37 // Estimate speed (counts/sec)
lddevrie 1:b33d4964669b 38 speed_ = -(count_-countp_)/0.00625*2.0*3.14159/(4.0*pulsesPerRev_);
lddevrie 0:1d5bf13d7dbb 39
lddevrie 0:1d5bf13d7dbb 40 // Age variable
lddevrie 0:1d5bf13d7dbb 41 countp_ = count_;
lddevrie 0:1d5bf13d7dbb 42 }
lddevrie 0:1d5bf13d7dbb 43
lddevrie 0:1d5bf13d7dbb 44
lddevrie 0:1d5bf13d7dbb 45 int EW305sbc::getCount(void)
lddevrie 0:1d5bf13d7dbb 46 {
lddevrie 0:1d5bf13d7dbb 47 return count_;
lddevrie 0:1d5bf13d7dbb 48 }
lddevrie 0:1d5bf13d7dbb 49
lddevrie 0:1d5bf13d7dbb 50 float EW305sbc::getSpeed(void)
lddevrie 0:1d5bf13d7dbb 51 {
lddevrie 0:1d5bf13d7dbb 52 return speed_;
lddevrie 0:1d5bf13d7dbb 53 }
lddevrie 0:1d5bf13d7dbb 54
lddevrie 2:6cf351721d86 55 float EW305sbc::getAngle(void)
lddevrie 2:6cf351721d86 56 {
lddevrie 2:6cf351721d86 57 return angle_;
lddevrie 2:6cf351721d86 58 }
lddevrie 0:1d5bf13d7dbb 59
lddevrie 0:1d5bf13d7dbb 60
lddevrie 0:1d5bf13d7dbb 61 //---- Function Listing -------------------------------
lddevrie 0:1d5bf13d7dbb 62
lddevrie 0:1d5bf13d7dbb 63 //----- LS7366 Encoder/Counter Routines --------------------
lddevrie 0:1d5bf13d7dbb 64 void EW305sbc::LS7366_cmd(int inst, int reg)
lddevrie 0:1d5bf13d7dbb 65 {
lddevrie 0:1d5bf13d7dbb 66 char cmd;
lddevrie 0:1d5bf13d7dbb 67 spi.format(8, 0);
lddevrie 0:1d5bf13d7dbb 68 spi.frequency(2000000);
lddevrie 0:1d5bf13d7dbb 69 cmd = (inst << 6) | (reg << 3);
lddevrie 0:1d5bf13d7dbb 70 // printf("\r\ncmd=0X%2X", cmd);
lddevrie 0:1d5bf13d7dbb 71 spi.write(cmd);
lddevrie 0:1d5bf13d7dbb 72 }
lddevrie 0:1d5bf13d7dbb 73
lddevrie 0:1d5bf13d7dbb 74 long EW305sbc::LS7366_read_counter(int chan_num)
lddevrie 0:1d5bf13d7dbb 75 {
lddevrie 0:1d5bf13d7dbb 76 union bytes {
lddevrie 0:1d5bf13d7dbb 77 char byte_enc[4];
lddevrie 0:1d5bf13d7dbb 78 long long_enc;
lddevrie 0:1d5bf13d7dbb 79 } counter;
lddevrie 0:1d5bf13d7dbb 80
lddevrie 0:1d5bf13d7dbb 81 counter.long_enc = 0;
lddevrie 0:1d5bf13d7dbb 82 spi.format(8, 0);
lddevrie 0:1d5bf13d7dbb 83 spi.frequency(2000000);
lddevrie 0:1d5bf13d7dbb 84
lddevrie 0:1d5bf13d7dbb 85 if(chan_num!=2) {
lddevrie 0:1d5bf13d7dbb 86 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 87 wait_us(1);
lddevrie 0:1d5bf13d7dbb 88 LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
lddevrie 0:1d5bf13d7dbb 89 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 90 wait_us(1);
lddevrie 0:1d5bf13d7dbb 91 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 92 } else {
lddevrie 0:1d5bf13d7dbb 93 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 94 wait_us(1);
lddevrie 0:1d5bf13d7dbb 95 LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
lddevrie 0:1d5bf13d7dbb 96 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 97 wait_us(1);
lddevrie 0:1d5bf13d7dbb 98 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 99 }
lddevrie 0:1d5bf13d7dbb 100 wait_us(1);
lddevrie 0:1d5bf13d7dbb 101 LS7366_cmd(RD,CNTR); //cmd = 0x60, READ from CNTR
lddevrie 0:1d5bf13d7dbb 102 counter.byte_enc[3] = spi.write(0x00);
lddevrie 0:1d5bf13d7dbb 103 counter.byte_enc[2] = spi.write(0x00);
lddevrie 0:1d5bf13d7dbb 104 counter.byte_enc[1] = spi.write(0x00);
lddevrie 0:1d5bf13d7dbb 105 counter.byte_enc[0] = spi.write(0x00);
lddevrie 0:1d5bf13d7dbb 106
lddevrie 0:1d5bf13d7dbb 107 if(chan_num!=2) {
lddevrie 0:1d5bf13d7dbb 108 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 109 } else {
lddevrie 0:1d5bf13d7dbb 110 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 111 }
lddevrie 0:1d5bf13d7dbb 112
lddevrie 0:1d5bf13d7dbb 113 return counter.long_enc; //return count
lddevrie 0:1d5bf13d7dbb 114 }
lddevrie 0:1d5bf13d7dbb 115
lddevrie 0:1d5bf13d7dbb 116 void EW305sbc::LS7366_quad_mode_x4(int chan_num)
lddevrie 0:1d5bf13d7dbb 117 {
lddevrie 0:1d5bf13d7dbb 118
lddevrie 0:1d5bf13d7dbb 119 spi.format(8, 0);
lddevrie 0:1d5bf13d7dbb 120 spi.frequency(2000000);
lddevrie 0:1d5bf13d7dbb 121
lddevrie 0:1d5bf13d7dbb 122 if(chan_num!=2) {
lddevrie 0:1d5bf13d7dbb 123 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 124 } else {
lddevrie 0:1d5bf13d7dbb 125 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 126 }
lddevrie 0:1d5bf13d7dbb 127 wait_us(1);
lddevrie 0:1d5bf13d7dbb 128 LS7366_cmd(WR,MDR0);// Write to the MDR0 register
lddevrie 0:1d5bf13d7dbb 129 wait_us(1);
lddevrie 0:1d5bf13d7dbb 130 spi.write(0x03); // X4 quadrature count mode
lddevrie 0:1d5bf13d7dbb 131 if(chan_num!=2) {
lddevrie 0:1d5bf13d7dbb 132 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 133 } else {
lddevrie 0:1d5bf13d7dbb 134 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 135 }
lddevrie 0:1d5bf13d7dbb 136 }
lddevrie 0:1d5bf13d7dbb 137
lddevrie 0:1d5bf13d7dbb 138 void EW305sbc::LS7366_reset_counter(int chan_num)
lddevrie 0:1d5bf13d7dbb 139 {
lddevrie 0:1d5bf13d7dbb 140 spi.format(8, 0); // set up SPI for 8 data bits, mode 0
lddevrie 0:1d5bf13d7dbb 141 spi.frequency(2000000); // 2MHz SPI clock
lddevrie 0:1d5bf13d7dbb 142
lddevrie 0:1d5bf13d7dbb 143 if(chan_num!=2) { // activate chip select
lddevrie 0:1d5bf13d7dbb 144 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 145 } else {
lddevrie 0:1d5bf13d7dbb 146 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 147 }
lddevrie 0:1d5bf13d7dbb 148 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 149 LS7366_cmd(CLR,CNTR); // Clear the counter register
lddevrie 0:1d5bf13d7dbb 150 if(chan_num!=2) { // de-activate chip select
lddevrie 0:1d5bf13d7dbb 151 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 152 } else {
lddevrie 0:1d5bf13d7dbb 153 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 154 }
lddevrie 0:1d5bf13d7dbb 155 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 156
lddevrie 0:1d5bf13d7dbb 157 if(chan_num!=2) { // activate chip select
lddevrie 0:1d5bf13d7dbb 158 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 159 } else {
lddevrie 0:1d5bf13d7dbb 160 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 161 }
lddevrie 0:1d5bf13d7dbb 162 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 163 LS7366_cmd(LOAD,CNTR); // load counter reg
lddevrie 0:1d5bf13d7dbb 164 if(chan_num!=2) { // de-activate chip select
lddevrie 0:1d5bf13d7dbb 165 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 166 } else {
lddevrie 0:1d5bf13d7dbb 167 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 168 }
lddevrie 0:1d5bf13d7dbb 169 }
lddevrie 0:1d5bf13d7dbb 170
lddevrie 0:1d5bf13d7dbb 171 void EW305sbc::LS7366_write_DTR(int chan_num, long enc_value)
lddevrie 0:1d5bf13d7dbb 172 {
lddevrie 0:1d5bf13d7dbb 173 union bytes { // Union to speed up byte writes
lddevrie 0:1d5bf13d7dbb 174 char byte_enc[4];
lddevrie 0:1d5bf13d7dbb 175 long long_enc;
lddevrie 0:1d5bf13d7dbb 176 } counter;
lddevrie 0:1d5bf13d7dbb 177
lddevrie 0:1d5bf13d7dbb 178 spi.format(8, 0); // set up SPI for 8 data bits, mode 0
lddevrie 0:1d5bf13d7dbb 179 spi.frequency(2000000); // 2MHz SPI clock
lddevrie 0:1d5bf13d7dbb 180
lddevrie 0:1d5bf13d7dbb 181 counter.long_enc = enc_value; // pass enc_value to Union
lddevrie 0:1d5bf13d7dbb 182
lddevrie 0:1d5bf13d7dbb 183 if(chan_num!=2) { // activate chip select
lddevrie 0:1d5bf13d7dbb 184 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 185 } else {
lddevrie 0:1d5bf13d7dbb 186 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 187 }
lddevrie 0:1d5bf13d7dbb 188 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 189 LS7366_cmd(WR,DTR); // Write to the Data Transfer Register
lddevrie 0:1d5bf13d7dbb 190 spi.write(counter.byte_enc[3]); // Write the 32-bit encoder value
lddevrie 0:1d5bf13d7dbb 191 spi.write(counter.byte_enc[2]);
lddevrie 0:1d5bf13d7dbb 192 spi.write(counter.byte_enc[1]);
lddevrie 0:1d5bf13d7dbb 193 spi.write(counter.byte_enc[0]);
lddevrie 0:1d5bf13d7dbb 194 if(chan_num!=2) { // de-activate the chip select
lddevrie 0:1d5bf13d7dbb 195 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 196 } else {
lddevrie 0:1d5bf13d7dbb 197 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 198 }
lddevrie 0:1d5bf13d7dbb 199
lddevrie 0:1d5bf13d7dbb 200 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 201 if(chan_num!=2) { // activate chip select
lddevrie 0:1d5bf13d7dbb 202 ls7166_cs1 = 0;
lddevrie 0:1d5bf13d7dbb 203 } else {
lddevrie 0:1d5bf13d7dbb 204 ls7166_cs2 = 0;
lddevrie 0:1d5bf13d7dbb 205 }
lddevrie 0:1d5bf13d7dbb 206 wait_us(1); // short delay
lddevrie 0:1d5bf13d7dbb 207 LS7366_cmd(LOAD,CNTR); // load command to the counter register from DTR
lddevrie 0:1d5bf13d7dbb 208 if(chan_num!=2) { // de-activate chip select
lddevrie 0:1d5bf13d7dbb 209 ls7166_cs1 = 1;
lddevrie 0:1d5bf13d7dbb 210 } else {
lddevrie 0:1d5bf13d7dbb 211 ls7166_cs2 = 1;
lddevrie 0:1d5bf13d7dbb 212 }
lddevrie 0:1d5bf13d7dbb 213 }