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@2:8154b75f836f, 2019-09-15 (annotated)

Committer:
Kirua
Date:
Sun Sep 15 17:39:58 2019 +0000
Revision:
2:8154b75f836f
Parent:
1:7826ec4d9405
Child:
3:d3694519bd73
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 2:8154b75f836f 8 encoder.frequency(4000000);
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 1:7826ec4d9405 26 float pre_angle = angle[num];
Kirua 1:7826ec4d9405 27 float _angle = 0;
Kirua 1:7826ec4d9405 28
Kirua 1:7826ec4d9405 29 uint8_t received = _SPI_T(num, 0x10); //read command
Kirua 1:7826ec4d9405 30 while (received != 0x10) { //loop while encoder is not ready to send
Kirua 1:7826ec4d9405 31 received = _SPI_T(num, 0x00);
Kirua 0:21ae825df645 32 }
Kirua 1:7826ec4d9405 33 temp[0][num] = _SPI_T(num, 0x00); //Recieve MSB
Kirua 1:7826ec4d9405 34 temp[1][num] = _SPI_T(num, 0x00); //recieve LSB
Kirua 1:7826ec4d9405 35 temp[0][num] &=~ 0xF0; //mask out the first 4 bits
Kirua 1:7826ec4d9405 36 EncoderByteData[num] = temp[0][num] << 8;
Kirua 1:7826ec4d9405 37 EncoderByteData[num] += temp[1][num];
Kirua 1:7826ec4d9405 38
Kirua 1:7826ec4d9405 39 if(!direction[num]) _angle = EncoderByteData[num] / 4096.0f * 2.0f * PI; //normal
Kirua 1:7826ec4d9405 40 else _angle = abs((EncoderByteData[num] / 4096.0f * 2.0f * PI) - 2*PI); //inverse
Kirua 1:7826ec4d9405 41 if(_angle > PI) angle[num] = _angle - 2*PI; //0~2π → -π~π
Kirua 1:7826ec4d9405 42 else angle[num] = _angle;
Kirua 1:7826ec4d9405 43
Kirua 1:7826ec4d9405 44 velocity[num] = (angle[num] - pre_angle) / delta_t;
Kirua 0:21ae825df645 45 }
Kirua 0:21ae825df645 46
Kirua 1:7826ec4d9405 47 bool SPI_Encoder::setZero(int num)
Kirua 0:21ae825df645 48 {
Kirua 1:7826ec4d9405 49 uint8_t received = _SPI_T(num, 0x70); //read command
Kirua 1:7826ec4d9405 50 while (received != 0x80) { //loop while encoder is not ready to send
Kirua 1:7826ec4d9405 51 received = _SPI_T(num, 0x00);
Kirua 1:7826ec4d9405 52 }
Kirua 1:7826ec4d9405 53 return true;
Kirua 0:21ae825df645 54 }
Kirua 0:21ae825df645 55
Kirua 1:7826ec4d9405 56 /* switch MSB or LSB */
Kirua 1:7826ec4d9405 57 uint8_t SPI_Encoder::_SPI_T (int num, uint8_t SPITransmit)
Kirua 1:7826ec4d9405 58 {
Kirua 1:7826ec4d9405 59 _switching(num, 0);
Kirua 1:7826ec4d9405 60 uint8_t SPI_temp = encoder.write(SPITransmit);
Kirua 1:7826ec4d9405 61 _switching(num, 1);
Kirua 2:8154b75f836f 62 wait_us(20); //Timmig is critical
Kirua 1:7826ec4d9405 63 return (SPI_temp);
Kirua 1:7826ec4d9405 64 }
Kirua 1:7826ec4d9405 65
Kirua 1:7826ec4d9405 66 /* switch read encoder */
Kirua 0:21ae825df645 67 void SPI_Encoder::_switching(int num, int value)
Kirua 0:21ae825df645 68 {
Kirua 1:7826ec4d9405 69 switch (num) {
Kirua 0:21ae825df645 70 case 0:
Kirua 0:21ae825df645 71 cs0 = value;
Kirua 0:21ae825df645 72 break;
Kirua 0:21ae825df645 73 case 1:
Kirua 0:21ae825df645 74 cs1 = value;
Kirua 0:21ae825df645 75 break;
Kirua 0:21ae825df645 76 case 2:
Kirua 0:21ae825df645 77 cs2 = value;
Kirua 0:21ae825df645 78 break;
Kirua 0:21ae825df645 79 case 3:
Kirua 0:21ae825df645 80 cs3 = value;
Kirua 0:21ae825df645 81 break;
Kirua 0:21ae825df645 82 default:
Kirua 0:21ae825df645 83 break;
Kirua 0:21ae825df645 84 }
Kirua 0:21ae825df645 85 }