Levi DeVries / EW305sbc

encoder and tachometer class for EW3XX single board computer

EW305sbc.cpp@5:58b13292286e, 2021-08-16 (annotated)

Committer:
lddevrie
Date:
Mon Aug 16 11:12:19 2021 +0000
Revision:
5:58b13292286e
Parent:
4:19ee7eb16605 
update for digital pot motor control

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 5:58b13292286e 6 : ch_(ch), pulsesPerRev_(pulsesPerRev), spi(p5, p6, p7), ls7166_cs1(p19,1), ls7166_cs2(p20,1), ad5293_sync(p12,1), ad5293_rdy(p27)
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 4:19ee7eb16605 13 dt_ = 0.001; // 500 Hz, also must update lines 19 and 38 since dt_ is not working for some reason??
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 4:19ee7eb16605 19 updater.attach(callback(this, &EW305sbc::recalc), 0.001); // 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 4:19ee7eb16605 38 speed_ = -(count_-countp_)/0.001*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 5:58b13292286e 213 }
lddevrie 5:58b13292286e 214
lddevrie 5:58b13292286e 215 // Write command and data - cmd[B13:B10], Data[B9:B0]
lddevrie 5:58b13292286e 216 void EW305sbc::write_ad5293(int cmd, int data)
lddevrie 5:58b13292286e 217 {
lddevrie 5:58b13292286e 218 spi.frequency(10000000); // Datasheet states up to 50MHz operation
lddevrie 5:58b13292286e 219 spi.format(16, 1); // 16 data bits, CPOL0, and CPHA1 (datasheet page 8)
lddevrie 5:58b13292286e 220 uint16_t dataw = (cmd << 10) | data; // set data write bits (command and data)
lddevrie 5:58b13292286e 221 ad5293_sync = 0;
lddevrie 5:58b13292286e 222 spi.write(dataw); // SPI transmit of 16 bits
lddevrie 5:58b13292286e 223
lddevrie 5:58b13292286e 224 int timeout = 0;
lddevrie 5:58b13292286e 225 while(!ad5293_rdy) { // takes about 250ns to update
lddevrie 5:58b13292286e 226 timeout++;
lddevrie 5:58b13292286e 227 if(timeout >1000) {
lddevrie 5:58b13292286e 228 //pc.printf("Timeout waiting for RDY wiper update! timout=%d\r\n\r\n", timeout);
lddevrie 5:58b13292286e 229 wait(2);
lddevrie 5:58b13292286e 230 }
lddevrie 5:58b13292286e 231 }
lddevrie 5:58b13292286e 232 ad5293_sync =1;
lddevrie 5:58b13292286e 233 spi.format(8,0);// return to 'normal' SPI format
lddevrie 5:58b13292286e 234 }
lddevrie 5:58b13292286e 235
lddevrie 5:58b13292286e 236
lddevrie 5:58b13292286e 237 //Motor control routine for digital potentiometer analog driver board
lddevrie 5:58b13292286e 238 // inp is analog voltage output (+/-1.0)
lddevrie 5:58b13292286e 239 void EW305sbc::analog_input(float inp)
lddevrie 5:58b13292286e 240 {
lddevrie 5:58b13292286e 241 if(inp>MAX_SCALE)
lddevrie 5:58b13292286e 242 inp=MAX_SCALE;
lddevrie 5:58b13292286e 243 if(inp<-MAX_SCALE)
lddevrie 5:58b13292286e 244 inp=-MAX_SCALE;
lddevrie 5:58b13292286e 245
lddevrie 5:58b13292286e 246 write_ad5293(WCON,0x3FF); // Write to Control Register, C2=1 (normal mode), C1=1 (allows update of wiper)
lddevrie 5:58b13292286e 247 write_ad5293(WRDAC,512 + inp*512);
lddevrie 5:58b13292286e 248 }
lddevrie 5:58b13292286e 249
lddevrie 5:58b13292286e 250 void EW305sbc::reset_ad5293()
lddevrie 5:58b13292286e 251 {
lddevrie 5:58b13292286e 252 write_ad5293(RESET,0x3FF);
lddevrie 5:58b13292286e 253 wait(2.5);
lddevrie 5:58b13292286e 254 LS7366_reset_counter(1);
lddevrie 5:58b13292286e 255 LS7366_quad_mode_x4(1);
lddevrie 5:58b13292286e 256 LS7366_write_DTR(1,0);
lddevrie 5:58b13292286e 257 }
lddevrie 5:58b13292286e 258
lddevrie 5:58b13292286e 259 void EW305sbc::init_ad5293()
lddevrie 5:58b13292286e 260 {
lddevrie 5:58b13292286e 261 write_ad5293(WCON,0x3FF); // Write to Control Register, C2=1 (normal mode), C1=1 (allows update of wiper)
lddevrie 5:58b13292286e 262 write_ad5293(RESET,0x3FF);
lddevrie 5:58b13292286e 263 wait(.4); // allow time for motor to settle if the mbed was just reset while motor was moving
lddevrie 5:58b13292286e 264 LS7366_reset_counter(1);
lddevrie 5:58b13292286e 265 LS7366_quad_mode_x4(1);
lddevrie 5:58b13292286e 266 LS7366_write_DTR(1,0);
lddevrie 5:58b13292286e 267
lddevrie 5:58b13292286e 268 write_ad5293(WCON,0x3FF); // Write to Control Register, C2=1 (normal mode), C1=1 (allows update of wiper)
lddevrie 5:58b13292286e 269 wait(1);
lddevrie 0:1d5bf13d7dbb 270 }