Skip to content

Commit 5de9220

Browse files
runger1101001runger1101001
authored
Merge pull request #45 from techy-robot/develop
Adding support for MA735
2 parents 79af273 + 9b895ce commit 5de9220

File tree

7 files changed

+572
-0
lines changed

7 files changed

+572
-0
lines changed

‎src/encoders/ma735/MA735.cpp

Lines changed: 297 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,297 @@
1+
#include "MA735.h"
2+
3+
MA735::MA735(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
4+
5+
};
6+
MA735::~MA735() {
7+
8+
};
9+
10+
void MA735::init(SPIClass* _spi) {
11+
spi = _spi;
12+
if (nCS >= 0) {
13+
pinMode(nCS, OUTPUT);
14+
digitalWrite(nCS, HIGH);
15+
}
16+
};
17+
18+
float MA735::getCurrentAngle() {
19+
return (readRawAngle() * _2PI)/MA735_16BIT;//It doesn't matter that it is divided by 65536, because the raw angle fills empty data bits with empty zeros so sensor resolution doesn't affect angle calculation
20+
}; // angle in radians, return current value
21+
22+
uint16_t MA735::readRawAngle() {
23+
uint16_t angle = transfer16(0x0000);
24+
return angle;
25+
}; // 9-13bit angle value
26+
27+
uint16_t MA735::getZero() {
28+
uint16_t result = readRegister(MA735_REG_ZERO_POSITION_MSB)<<8;
29+
result |= readRegister(MA735_REG_ZERO_POSITION_LSB);
30+
return result;
31+
};
32+
uint8_t MA735::getBiasCurrentTrimming() {
33+
return readRegister(MA735_REG_BCT);
34+
};
35+
bool MA735::isBiasCurrrentTrimmingX() {
36+
return (readRegister(MA735_REG_ET) & 0x01)==0x01;
37+
};
38+
bool MA735::isBiasCurrrentTrimmingY() {
39+
return (readRegister(MA735_REG_ET) & 0x02)==0x02;
40+
};
41+
uint16_t MA735::getPulsesPerTurn() {
42+
uint16_t result = readRegister(MA735_REG_ILIP_PPT_LSB)>>6;
43+
result |= ((uint16_t)readRegister(MA735_REG_PPT_MSB))<<2;
44+
return result+1;
45+
};
46+
uint8_t MA735::getIndexLength() {
47+
return (readRegister(MA735_REG_ILIP_PPT_LSB)>>2)&0x0F;
48+
};
49+
uint8_t MA735::getRotationDirection() {
50+
return (readRegister(MA735_REG_RD)>>7);
51+
};
52+
uint8_t MA735::getFieldStrengthHighThreshold() {
53+
return (readRegister(MA735_REG_MGLT_MGHT)>>2)&0x07;
54+
};
55+
uint8_t MA735::getFieldStrengthLowThreshold() {
56+
return (readRegister(MA735_REG_MGLT_MGHT)>>5)&0x07;
57+
};
58+
FieldStrength MA735::getFieldStrength() {
59+
return (FieldStrength)(readRegister(MA735_REG_MGH_MGL)>>6);
60+
};
61+
uint8_t MA735::getFilterWindow() {
62+
return readRegister(MA735_REG_FW);
63+
};
64+
uint8_t MA735::getHysteresis() {
65+
return readRegister(MA735_REG_HYS);
66+
};
67+
float MA735::getResolution() {
68+
//All I could find in the datasheet was a table with the correlation, no function to convert Filter window to res.
69+
uint8_t reg = readRegister(MA735_REG_FW);
70+
float result;
71+
switch (reg) {
72+
case 51:
73+
result = 9.0;
74+
break;
75+
case 68:
76+
result = 9.5;
77+
break;
78+
case 85:
79+
result = 10.0;
80+
break;
81+
case 102:
82+
result = 10.5;
83+
break;
84+
case 119:
85+
result = 11.0;
86+
break;
87+
case 136:
88+
result = 11.5;
89+
break;
90+
case 153:
91+
result = 12.0;
92+
break;
93+
case 170:
94+
result = 12.5;
95+
break;
96+
case 187:
97+
result = 13.0;
98+
break;
99+
default:
100+
result = 11.0;
101+
break;
102+
}
103+
return result;
104+
};
105+
int MA735::getUpdateTime() {
106+
//All I could find in the datasheet was a table with the correlation, no function to convert Filter window to update time.
107+
//Returns result in microseconds
108+
uint8_t reg = readRegister(MA735_REG_FW);
109+
int result;
110+
switch (reg) {
111+
case 51:
112+
result = 64;
113+
break;
114+
case 68:
115+
result = 128;
116+
break;
117+
case 85:
118+
result = 256;
119+
break;
120+
case 102:
121+
result = 512;
122+
break;
123+
case 119:
124+
result = 1024;
125+
break;
126+
case 136:
127+
result = 2048;
128+
break;
129+
case 153:
130+
result = 4096;
131+
break;
132+
case 170:
133+
result = 8192;
134+
break;
135+
case 187:
136+
result = 16384;
137+
break;
138+
default:
139+
result = 1024;
140+
break;
141+
}
142+
return result;
143+
};
144+
145+
146+
147+
void MA735::setZero(uint16_t value) {
148+
writeRegister(MA735_REG_ZERO_POSITION_MSB, value>>8);
149+
writeRegister(MA735_REG_ZERO_POSITION_LSB, value&0x00FF);
150+
};
151+
void MA735::setBiasCurrentTrimming(uint8_t value) {
152+
writeRegister(MA735_REG_BCT, value);
153+
};
154+
void MA735::setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled) {
155+
uint8_t val = Xenabled ? 0x01 : 0x00;
156+
val |= (Yenabled ? 0x02 : 0x00);
157+
writeRegister(MA735_REG_ET, val);
158+
};
159+
void MA735::setPulsesPerTurn(uint16_t value) {
160+
uint16_t pptVal = value - 1;
161+
writeRegister(MA735_REG_PPT_MSB, pptVal>>2);
162+
uint8_t val = readRegister(MA735_REG_ILIP_PPT_LSB);
163+
val &= 0x3F;
164+
val |= (pptVal&0x03)<<6;
165+
writeRegister(MA735_REG_ILIP_PPT_LSB, val);
166+
};
167+
void MA735::setIndexLength(uint8_t value) {
168+
uint8_t val = readRegister(MA735_REG_ILIP_PPT_LSB);
169+
val &= 0xC0;
170+
val |= ((value<<2)&0x3F);
171+
writeRegister(MA735_REG_ILIP_PPT_LSB, val);
172+
};
173+
void MA735::setRotationDirection(uint8_t value) {
174+
if (value==0)
175+
writeRegister(MA735_REG_RD, 0x00);
176+
else
177+
writeRegister(MA735_REG_RD, 0x80);
178+
};
179+
void MA735::setFieldStrengthThresholds(uint8_t high, uint8_t low) {
180+
uint8_t val = (low<<5) | (high<<2);
181+
writeRegister(MA735_REG_MGLT_MGHT, val);
182+
};
183+
void MA735::setFilterWindow(uint8_t value) {
184+
writeRegister(MA735_REG_FW, value);
185+
};
186+
void MA735::setHysteresis(uint8_t value) {
187+
writeRegister(MA735_REG_HYS, value);
188+
};
189+
void MA735::setResolution(float res) {
190+
//All I could find in the datasheet was a table with the correlation, no function to convert Filter window to res.
191+
uint8_t value;
192+
uint8_t res_int = res * 10;//It has to be a basic type for the switch case
193+
switch (res_int) {
194+
case 90:
195+
value = 51;
196+
break;
197+
case 95:
198+
value = 68;
199+
break;
200+
case 100:
201+
value = 85;
202+
break;
203+
case 105:
204+
value = 102;
205+
break;
206+
case 110:
207+
value = 119;
208+
break;
209+
case 115:
210+
value = 136;
211+
break;
212+
case 120:
213+
value = 153;
214+
break;
215+
case 125:
216+
value = 170;
217+
break;
218+
case 130:
219+
value = 187;
220+
break;
221+
default:
222+
value = 119;
223+
break;
224+
}
225+
writeRegister(MA735_REG_FW, value);
226+
};
227+
void MA735::setUpdateTime(int microsec) {
228+
//All I could find in the datasheet was a table with the correlation, no function to convert Filter window to update time.
229+
//time in microseconds
230+
uint8_t value;
231+
switch (microsec) {
232+
case 64:
233+
value = 51;
234+
break;
235+
case 128:
236+
value = 68;
237+
break;
238+
case 256:
239+
value = 85;
240+
break;
241+
case 512:
242+
value = 102;
243+
break;
244+
case 1024:
245+
value = 119;
246+
break;
247+
case 2048:
248+
value = 136;
249+
break;
250+
case 4096:
251+
value = 153;
252+
break;
253+
case 8192:
254+
value = 170;
255+
break;
256+
case 16384:
257+
value = 187;
258+
break;
259+
default:
260+
value = 119;
261+
break;
262+
}
263+
writeRegister(MA735_REG_FW, value);
264+
};
265+
266+
267+
uint16_t MA735::transfer16(uint16_t outValue) {
268+
spi->beginTransaction(settings);
269+
if (nCS >= 0)
270+
digitalWrite(nCS, LOW);
271+
uint16_t value = spi->transfer16(outValue);
272+
if (nCS >= 0)
273+
digitalWrite(nCS, HIGH);
274+
spi->endTransaction();
275+
return value;
276+
};
277+
uint8_t MA735::readRegister(uint8_t reg) {
278+
uint16_t cmd = 0x4000 | ((reg&0x001F)<<8);
279+
uint16_t value = transfer16(cmd);
280+
delayMicroseconds(1);
281+
value = transfer16(0x0000);
282+
return value>>8;
283+
};
284+
uint8_t MA735::writeRegister(uint8_t reg, uint8_t value) {
285+
uint8_t write_check = readRegister(reg);
286+
//no need to rewrite, it is the exact same value
287+
if (write_check == value) {
288+
return write_check;
289+
}
290+
else {
291+
uint16_t cmd = 0x8000 | ((reg&0x1F)<<8) | value;
292+
uint16_t result = transfer16(cmd);
293+
delay(20); // 20ms delay required
294+
result = transfer16(0x0000);
295+
return result>>8;
296+
}
297+
};

‎src/encoders/ma735/MA735.h

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
#ifndef __MA735_H__
2+
#define __MA735_H__
3+
4+
5+
#include "Arduino.h"
6+
#include "SPI.h"
7+
8+
enum FieldStrength : uint8_t {
9+
FS_NORMAL = 0x00,
10+
FS_LOW = 0x01,
11+
FS_HIGH = 0x02,
12+
FS_ERR = 0x03 // impossible state
13+
};
14+
15+
16+
#define _2PI 6.28318530718f
17+
#define MA735_16BIT 65536.0f
18+
19+
#define MA735_REG_ZERO_POSITION_LSB 0x00
20+
#define MA735_REG_ZERO_POSITION_MSB 0x01
21+
#define MA735_REG_BCT 0x02
22+
#define MA735_REG_ET 0x03
23+
#define MA735_REG_ILIP_PPT_LSB 0x04
24+
#define MA735_REG_PPT_MSB 0x05
25+
#define MA735_REG_MGLT_MGHT 0x06
26+
#define MA735_REG_RD 0x09
27+
#define MA735_REG_FW 0x0E
28+
#define MA735_REG_HYS 0x10
29+
#define MA735_REG_MGH_MGL 0x1B
30+
31+
#define MA735_BITORDER MSBFIRST
32+
33+
static SPISettings MA735SPISettings(1000000, MA735_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
34+
static SPISettings MA735SSISettings(4000000, MA735_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
35+
36+
37+
class MA735 {
38+
public:
39+
MA735(SPISettings settings = MA735SPISettings, int nCS = -1);
40+
virtual ~MA735();
41+
42+
virtual void init(SPIClass* _spi = &SPI);
43+
44+
float getCurrentAngle(); // angle in radians, return current value
45+
46+
uint16_t readRawAngle(); // 9-13bit angle value
47+
uint16_t readRawAngleSSI(); // 9-13bit angle value
48+
49+
uint16_t getZero();
50+
uint8_t getBiasCurrentTrimming();
51+
bool isBiasCurrrentTrimmingX();
52+
bool isBiasCurrrentTrimmingY();
53+
uint16_t getPulsesPerTurn();
54+
uint8_t getIndexLength();
55+
uint8_t getRotationDirection();
56+
uint8_t getFieldStrengthHighThreshold();
57+
uint8_t getFieldStrengthLowThreshold();
58+
FieldStrength getFieldStrength();
59+
uint8_t getFilterWindow();
60+
uint8_t getHysteresis();
61+
float getResolution();
62+
int getUpdateTime();
63+
64+
void setZero(uint16_t);
65+
void setBiasCurrentTrimming(uint8_t);
66+
void setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled);
67+
void setPulsesPerTurn(uint16_t);
68+
void setIndexLength(uint8_t);
69+
void setRotationDirection(uint8_t);
70+
void setFieldStrengthThresholds(uint8_t high, uint8_t low);
71+
void setFilterWindow(uint8_t);
72+
void setHysteresis(uint8_t);
73+
void setResolution(float resolution);
74+
void setUpdateTime(int microsec);
75+
76+
private:
77+
SPIClass* spi;
78+
SPISettings settings;
79+
int nCS = -1;
80+
//float MA735_CPR = 65536.0f;
81+
82+
uint16_t transfer16(uint16_t outValue);
83+
uint8_t readRegister(uint8_t reg);
84+
uint8_t writeRegister(uint8_t reg, uint8_t value);
85+
};
86+
87+
#endif

0 commit comments

Comments
 (0)