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@5:1458a8f024dc, 2019-09-26 (annotated)

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