ichinoseki_Bteam_2019 / SPI_Encoder

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ArmNode

SPI_Encoder.cpp@3:d3694519bd73, 2019-09-19 (annotated)

Committer:
Kirua
Date:
Thu Sep 19 13:00:42 2019 +0000
Revision:
3:d3694519bd73
Parent:
2:8154b75f836f
Child:
4:2331b7fcfe8d
fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kirua 0:21ae825df645 1 #include "mbed.h"
Kirua 0:21ae825df645 2 #include "SPI_Encoder.h"
Kirua 0:21ae825df645 3
Kirua 0:21ae825df645 4
Kirua 1:7826ec4d9405 5 SPI_Encoder::SPI_Encoder(PinName mosi, PinName miso, PinName sclk, PinName _cs0, PinName _cs1, PinName _cs2, PinName _cs3, float t) : encoder(mosi, miso, sclk), cs0(_cs0), cs1(_cs1), cs2(_cs2), cs3(_cs3)
Kirua 0:21ae825df645 6 {
Kirua 1:7826ec4d9405 7 encoder.format(8,1);
Kirua 3:d3694519bd73 8 encoder.frequency(2000000);
Kirua 1:7826ec4d9405 9 delta_t = t;
Kirua 1:7826ec4d9405 10 for(int i = 0; i < encoder_num; i++)
Kirua 1:7826ec4d9405 11 direction[i] = 0;
Kirua 1:7826ec4d9405 12 cs0 = 1;
Kirua 1:7826ec4d9405 13 cs1 = 1;
Kirua 1:7826ec4d9405 14 cs2 = 1;
Kirua 1:7826ec4d9405 15 cs3 = 1;
Kirua 1:7826ec4d9405 16 wait(0.1); //encoder init
Kirua 0:21ae825df645 17 };
Kirua 0:21ae825df645 18
Kirua 1:7826ec4d9405 19 void SPI_Encoder::inverse(int num)
Kirua 1:7826ec4d9405 20 {
Kirua 1:7826ec4d9405 21 direction[num] = 1;
Kirua 1:7826ec4d9405 22 }
Kirua 1:7826ec4d9405 23
Kirua 0:21ae825df645 24 void SPI_Encoder::getPosition(int num)
Kirua 0:21ae825df645 25 {
Kirua 3:d3694519bd73 26 uint8_t temp[2];
Kirua 3:d3694519bd73 27 uint16_t EncoderByteData;
Kirua 1:7826ec4d9405 28 float pre_angle = angle[num];
Kirua 1:7826ec4d9405 29 float _angle = 0;
Kirua 1:7826ec4d9405 30
Kirua 1:7826ec4d9405 31 uint8_t received = _SPI_T(num, 0x10); //read command
Kirua 1:7826ec4d9405 32 while (received != 0x10) { //loop while encoder is not ready to send
Kirua 1:7826ec4d9405 33 received = _SPI_T(num, 0x00);
Kirua 0:21ae825df645 34 }
Kirua 3:d3694519bd73 35 temp[0] = _SPI_T(num, 0x00); //Recieve MSB
Kirua 3:d3694519bd73 36 temp[1] = _SPI_T(num, 0x00); //recieve LSB
Kirua 3:d3694519bd73 37 temp[0] &=~ 0xF0; //mask out the first 4 bits
Kirua 3:d3694519bd73 38 EncoderByteData = temp[0] << 8;
Kirua 3:d3694519bd73 39 EncoderByteData += temp[1];
Kirua 1:7826ec4d9405 40
Kirua 3:d3694519bd73 41 if(!direction[num]) _angle = EncoderByteData / 4096.0f * 2.0f * PI; //normal
Kirua 3:d3694519bd73 42 else _angle = abs((EncoderByteData / 4096.0f * 2.0f * PI) - 2*PI); //inverse
Kirua 1:7826ec4d9405 43 if(_angle > PI) angle[num] = _angle - 2*PI; //0~2π → -π~π
Kirua 1:7826ec4d9405 44 else angle[num] = _angle;
Kirua 1:7826ec4d9405 45
Kirua 1:7826ec4d9405 46 velocity[num] = (angle[num] - pre_angle) / delta_t;
Kirua 0:21ae825df645 47 }
Kirua 0:21ae825df645 48
Kirua 1:7826ec4d9405 49 bool SPI_Encoder::setZero(int num)
Kirua 0:21ae825df645 50 {
Kirua 1:7826ec4d9405 51 uint8_t received = _SPI_T(num, 0x70); //read command
Kirua 1:7826ec4d9405 52 while (received != 0x80) { //loop while encoder is not ready to send
Kirua 1:7826ec4d9405 53 received = _SPI_T(num, 0x00);
Kirua 1:7826ec4d9405 54 }
Kirua 1:7826ec4d9405 55 return true;
Kirua 0:21ae825df645 56 }
Kirua 0:21ae825df645 57
Kirua 1:7826ec4d9405 58 /* switch MSB or LSB */
Kirua 1:7826ec4d9405 59 uint8_t SPI_Encoder::_SPI_T (int num, uint8_t SPITransmit)
Kirua 1:7826ec4d9405 60 {
Kirua 1:7826ec4d9405 61 _switching(num, 0);
Kirua 1:7826ec4d9405 62 uint8_t SPI_temp = encoder.write(SPITransmit);
Kirua 1:7826ec4d9405 63 _switching(num, 1);
Kirua 2:8154b75f836f 64 wait_us(20); //Timmig is critical
Kirua 1:7826ec4d9405 65 return (SPI_temp);
Kirua 1:7826ec4d9405 66 }
Kirua 1:7826ec4d9405 67
Kirua 1:7826ec4d9405 68 /* switch read encoder */
Kirua 0:21ae825df645 69 void SPI_Encoder::_switching(int num, int value)
Kirua 0:21ae825df645 70 {
Kirua 1:7826ec4d9405 71 switch (num) {
Kirua 0:21ae825df645 72 case 0:
Kirua 0:21ae825df645 73 cs0 = value;
Kirua 0:21ae825df645 74 break;
Kirua 0:21ae825df645 75 case 1:
Kirua 0:21ae825df645 76 cs1 = value;
Kirua 0:21ae825df645 77 break;
Kirua 0:21ae825df645 78 case 2:
Kirua 0:21ae825df645 79 cs2 = value;
Kirua 0:21ae825df645 80 break;
Kirua 0:21ae825df645 81 case 3:
Kirua 0:21ae825df645 82 cs3 = value;
Kirua 0:21ae825df645 83 break;
Kirua 0:21ae825df645 84 default:
Kirua 0:21ae825df645 85 break;
Kirua 0:21ae825df645 86 }
Kirua 0:21ae825df645 87 }