1 | public final class MotorController {
|
2 | private static final int IC_STAT = 0x00;
|
3 | private static final int OCP_STAT_1 = 0x01;
|
4 | private static final int OCP_STAT_2 = 0x02;
|
5 | private static final int OCP_STAT_3 = 0x03;
|
6 | private static final int OLD_STAT_1 = 0x04;
|
7 | private static final int OLD_STAT_2 = 0x05;
|
8 | private static final int OLD_STAT_3 = 0x06;
|
9 |
|
10 | private static final int CONFIG_CTRL = 0x07;
|
11 | private static final int OP_CTRL_1 = 0x08;
|
12 | private static final int OP_CTRL_2 = 0x09;
|
13 | private static final int OP_CTRL_3 = 0x0A;
|
14 | private static final int PWM_CTRL_1 = 0x0B;
|
15 | private static final int PWM_CTRL_2 = 0x0C;
|
16 | private static final int FW_CTRL_1 = 0x0D;
|
17 | private static final int FW_CTRL_2 = 0x0E;
|
18 | private static final int PWM_MAP_CTRL_1 = 0x0F;
|
19 | private static final int PWM_MAP_CTRL_2 = 0x10;
|
20 | private static final int PWM_MAP_CTRL_3 = 0x11;
|
21 | private static final int PWM_FREQ_CTRL = 0x12;
|
22 | private static final int PWM_DUTY_CTRL_1 = 0x13;
|
23 | private static final int PWM_DUTY_CTRL_2 = 0x14;
|
24 | private static final int PWM_DUTY_CTRL_3 = 0x15;
|
25 | private static final int PWM_DUTY_CTRL_4 = 0x16;
|
26 | private static final int SR_CTRL_1 = 0x17;
|
27 | private static final int SR_CTRL_2 = 0x18;
|
28 | private static final int OLD_CTRL_1 = 0x19;
|
29 | private static final int OLD_CTRL_2 = 0x1A;
|
30 | private static final int OLD_CTRL_3 = 0x1B;
|
31 | private static final int OLD_CTRL_4 = 0x24;
|
32 |
|
33 | private final Spi drv0, drv1;
|
34 | private final DigitalInput fault1, fault2;
|
35 |
|
36 | public MotorController(Context pi4j) {
|
37 | fault1 = pi4j.create(DigitalInput.newConfigBuilder(pi4j).address(24).pull(PullResistance.PULL_UP));
|
38 | fault2 = pi4j.create(DigitalInput.newConfigBuilder(pi4j).address(25).pull(PullResistance.PULL_UP));
|
39 |
|
40 | final int spiClockFreq = 1_000_000; // Hz
|
41 | final SpiMode mode = SpiMode.MODE_1;
|
42 | drv0 = pi4j.create(Spi.newConfigBuilder(pi4j).bus(SpiBus.BUS_0).chipSelect(SpiChipSelect.CS_0).mode(mode).baud(spiClockFreq));
|
43 | drv1 = pi4j.create(Spi.newConfigBuilder(pi4j).bus(SpiBus.BUS_0).chipSelect(SpiChipSelect.CS_1).mode(mode).baud(spiClockFreq));
|
44 |
|
45 | initializeChip(drv0);
|
46 | initializeChip(drv1);
|
47 | }
|
48 |
|
49 | private void initializeChip(Spi spi) {
|
50 | write(spi, OP_CTRL_1, (byte) 0x00);
|
51 | write(spi, OP_CTRL_2, (byte) 0x00);
|
52 | write(spi, OP_CTRL_3, (byte) 0x00);
|
53 |
|
54 | write(spi, OLD_CTRL_1, (byte) 0x00);
|
55 | write(spi, OLD_CTRL_2, (byte) 0x00);
|
56 | write(spi, OLD_CTRL_3, (byte) 0x00);
|
57 | write(spi, OLD_CTRL_4, (byte) 0x00);
|
58 |
|
59 | write(spi, CONFIG_CTRL, (byte) 0x81);
|
60 |
|
61 | write(spi, PWM_FREQ_CTRL, (byte) 0xAA); // 20kHz
|
62 | write(spi, SR_CTRL_1, (byte) 0x00);
|
63 | write(spi, SR_CTRL_2, (byte) 0x00);
|
64 |
|
65 | write(spi, CONFIG_CTRL, (byte) 0x81);
|
66 |
|
67 | System.out.print("PWM_FREQ_CTRL: ");
|
68 | printBytes(read(spi, PWM_FREQ_CTRL));
|
69 | }
|
70 |
|
71 | private byte write(Spi spi, int registerAddress, byte value) {
|
72 | byte command = (byte) (64 | (registerAddress & 0x3F));
|
73 | byte[] readBuffer = new byte[2];
|
74 | spi.transfer(new byte[] { command, value }, readBuffer);
|
75 | return readBuffer[1];
|
76 | }
|
77 |
|
78 | private byte read(Spi spi, int registerAddress) {
|
79 | return write(spi, registerAddress, (byte) 0);
|
80 | }
|
81 | }
|