RobotCore 1.0
WPILib port to raspberry Pi
Loading...
Searching...
No Matches
Device.java
1/*
2 * Copyright (C) 2016 John H. Gaby
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, version 3 of the License.
7 *
8 * This program is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * Contact: robotics@gabysoft.com
17 */
18
19package robotCore;
20
21// import java.io.IOException;
22import java.nio.ByteBuffer;
23import java.util.ArrayList;
24import java.util.Collections;
25import java.util.List;
26
27
28
29import robotCore.TwoWire.HardwareException;
30import robotCore.TwoWire.RunCommand;
31
32//import robot.Robot;
33
40public class Device {
41 public enum PinType {
42 PWMPin,
43 OutputPin,
44 InterruptPin,
45 InputPin,
46 AnalogPin
47 }
48
49 public static boolean PCTest = false;
50
51 public final static int PA0 = 0;
52 public final static int PA1 = 1;
53 public final static int PA2 = 2;
54 public final static int PA3 = 3;
55 public final static int PA4 = 4;
56 public final static int PA5 = 5;
57 public final static int PA6 = 6;
58 public final static int PA7 = 7;
59 public final static int PA8 = 8;
60 public final static int PA9 = 9;
61 public final static int PA10 = 10;
62 public final static int PA11 = 11;
63 public final static int PA12 = 12;
64 public final static int PA13 = 13;
65 public final static int PA14 = 14;
66 public final static int PA15 = 15;
67
68 public final static int PB0 = 16;
69 public final static int PB1 = 17;
70 public final static int PB2 = 18;
71 public final static int PB3 = 19;
72 public final static int PB4 = 20;
73 public final static int PB5 = 21;
74 public final static int PB6 = 22;
75 public final static int PB7 = 23;
76 public final static int PB8 = 24;
77 public final static int PB9 = 25;
78 public final static int PB10 = 26;
79 public final static int PB11 = 27;
80 public final static int PB12 = 28;
81 public final static int PB13 = 29;
82 public final static int PB14 = 30;
83 public final static int PB15 = 31;
84
85 public final static int PC13 = 32;
86 public final static int PC14 = 33;
87 public final static int PC15 = 34;
88
89 private static class V1 {
90
91 // public final static int Q1_INT = PB3;
92 // public final static int Q1_DIR = PA15;
93 // public final static int Q2_INT = PB4;
94 // public final static int Q2_DIR = PA12;
95 // public final static int Q3_INT = PB5;
96 // public final static int Q3_DIR = PC13;
97 public final static int Q4_INT = PB1;
98 public final static int Q4_DIR = PC14;
99 // public final static int Q5_INT = PB0;
100 public final static int Q5_DIR = PC15;
101
102 public final static int M1_1_DIR = PA4;
103 // public final static int M1_1_PWM = PA1;
104 public final static int M1_2_DIR = PA5;
105 // public final static int M1_2_PWM = PA0;
106 // public final static int M2_1_DIR = PB15;
107 // public final static int M2_1_PWM = PA3;
108 // public final static int M2_2_DIR = PB14;
109 // public final static int M2_2_PWM = PA2;
110 // public final static int M3_1_DIR = PB13;
111 // public final static int M3_1_PWM = PA7;
112 public final static int M3_2_DIR = PB12;
113 // public final static int M3_2_PWM = PA6;
114
115 public final static int IO_1 = PA8;
116 public final static int IO_2 = PA9;
117 public final static int IO_3 = PA10;
118 public final static int IO_4 = PA11;
119
120 public final static int A1_A = PA0;
121 public final static int A1_B = PA1;
122 public final static int A2_A = PA2;
123 public final static int A2_B = PA3;
124 }
125
126 public static class V3 {
127 public final static int PC0 = 32;
128 public final static int PC1 = 33;
129 public final static int PC2 = 34;
130 public final static int PC3 = 35;
131 public final static int PC4 = 36;
132 public final static int PC5 = 37;
133 public final static int PC6 = 38;
134 public final static int PC7 = 39;
135 public final static int PC8 = 40;
136 public final static int PC9 = 41;
137 public final static int PC10 = 42;
138 public final static int PC11 = 43;
139 public final static int PC12 = 44;
140 public final static int PC13 = 45;
141 public final static int PC14 = 46;
142 public final static int PC15 = 47;
143
144 public final static int PD2 = 48;
145
146 public final static int M1_1_PWM = Device.V3.PC6;
147 public final static int M1_1_DIR = Device.V3.PC3;
148 public final static int M1_2_PWM = Device.V3.PC7;
149 public final static int M1_2_DIR = Device.V3.PC2;
150
151 public final static int M2_1_PWM = Device.V3.PC8;
152 public final static int M2_1_DIR = Device.V3.PC1;
153 public final static int M2_2_PWM = Device.V3.PC9;
154 public final static int M2_2_DIR = Device.V3.PC0;
155
156 public final static int M3_1_PWM = Device.PA8;
157 public final static int M3_1_DIR = Device.PB9;
158 public final static int M3_2_PWM = Device.PA9;
159 public final static int M3_2_DIR = Device.PB8;
160
161 public final static int M4_1_PWM = Device.PA10;
162 public final static int M4_1_DIR = Device.PB5;
163 public final static int M4_2_PWM = Device.PA11;
164 public final static int M4_2_DIR = Device.PB4;
165
166 public final static int M5_1_PWM = Device.PA15;
167 public final static int M5_1_DIR = Device.V3.PC12;
168 public final static int M5_2_PWM = Device.PB3;
169 public final static int M5_2_DIR = Device.PA12;
170
171 public final static int Q1_INT = Device.PB0;
172 public final static int Q1_DIR = Device.V3.PD2;
173
174 public final static int Q2_INT = Device.PB1;
175 public final static int Q2_DIR = Device.V3.PC15;
176
177 public final static int Q3_INT = Device.PB2;
178 public final static int Q3_DIR = Device.V3.PC14;
179
180 public final static int Q4_INT = Device.PB12;
181 public final static int Q4_DIR = Device.V3.PC13;
182
183 public final static int Q5_INT = Device.PB13;
184 public final static int Q5_DIR = Device.V3.PC4;
185
186 public final static int Q6_INT = Device.PB14;
187 public final static int Q6_DIR = Device.V3.PC5;
188
189 public final static int A1_A = Device.PA0;
190 public final static int A1_B = Device.PA7;
191
192 public final static int A2_A = Device.PA1;
193 public final static int A2_B = Device.PA6;
194
195 public final static int A3_A = Device.PA2;
196 public final static int A3_B = Device.PA5;
197
198 public final static int A4_A = Device.PA3;
199 public final static int A4_B = Device.PA4;
200
201 public final static int IO1_INT = Device.PB15;
202 }
203
204 public static class V4 {
205 public final static int PA0 = 000;
206 public final static int PA1 = 001;
207
208 public final static int PC0 = 020;
209 public final static int PC1 = 021;
210 public final static int PC2 = 022;
211 public final static int PC3 = 023;
212 public final static int PC4 = 024;
213 public final static int PC5 = 025;
214 public final static int PC6 = 026;
215 public final static int PC7 = 027;
216
217 public final static int PD0 = 030;
218 public final static int PD1 = 031;
219 public final static int PD2 = 032;
220 public final static int PD3 = 033;
221 public final static int PD4 = 034;
222 public final static int PD5 = 035;
223 public final static int PD6 = 036;
224 public final static int PD7 = 037;
225
226 public final static int M1_1_PWM = PC3;
227 public final static int M1_1_DIR = PC6;
228 public final static int M1_2_PWM = PC4;
229 public final static int M1_2_DIR = PC7;
230
231 public final static int Q1_INT = PD3;
232 public final static int Q1_DIR = PC5;
233 public final static int Q2_INT = PD4;
234 public final static int Q2_DIR = PC0;
235
236 public final static int IO = PD6;
237 }
238
243 public static int M1_1_DIR = Device.PA8; // Shared with IO-4
247 public static final int M1_1_PWM = Device.PA1;
252 public static int M1_2_DIR = Device.PA9; // Shared with IO-3
256 public static final int M1_2_PWM = Device.PA0;
260 public static final int M2_1_DIR = Device.PB15;
264 public static final int M2_1_PWM = Device.PA3;
268 public static final int M2_2_DIR = Device.PB14;
272 public static final int M2_2_PWM = Device.PA2;
276 public static final int M3_1_DIR = Device.PB13;
280 public static final int M3_1_PWM = Device.PA7;
284 public static int M3_2_DIR = Device.PC14;
288 public static final int M3_2_PWM = Device.PA6;
289
293 public static int A1_A = Device.PA4;
297 public static int A1_B = Device.PA5;
302 public static int A2_A = Device.PB0; // Shared with Q5
307 public static int A2_B = Device.PB1; // Shared with Q5
308
312 public static final int Q1_INT = Device.PB3;
316 public static final int Q1_DIR = Device.PA15;
320 public static final int Q2_INT = Device.PB4;
324 public static final int Q2_DIR = Device.PA12;
325
329 public static final int Q3_INT = Device.PB5;
333 public static final int Q3_DIR = Device.PC13;
334
338 public static int Q4_INT = Device.PB12;
342 public static int Q4_DIR = Device.PC15;
347 public static final int Q5_INT = Device.PB0; // Shared with A2
352 public static int Q5_DIR = Device.PB1; // Shared with A2
353
357 public static int IO_1 = PA11;
361 public static int IO_2 = PA10;
366 public static int IO_3 = PA9; // Shared with M1-1
371 public static int IO_4 = PA8; // Shared with M1-2
372
373 private static void setVersion1() {
374 Logger.log("Device", 1, "setVersion1()");
375 // public final static int Q4_INT = PB1;
376 Q4_INT = V1.Q4_INT;
377 // public final static int Q4_DIR = PC14;
378 Q4_DIR = V1.Q4_DIR;
379 // public final static int Q5_DIR = PC15;
380 Q5_DIR = V1.Q5_DIR;
381
382 // public final static int M1_1_DIR = PA4;
383 M1_1_DIR = V1.M1_1_DIR;
384 // public final static int M1_2_DIR = PA5;
385 M1_2_DIR = V1.M1_2_DIR;
386 // public final static int M3_2_DIR = PB12;
387 M3_2_DIR = V1.M3_2_DIR;
388
389 // public final static int IO_1 = PA8;
390 IO_1 = V1.IO_1;
391 // public final static int IO_2 = PA9;
392 IO_2 = V1.IO_2;
393 // public final static int IO_3 = PA10;
394 IO_3 = V1.IO_3;
395 // public final static int IO_4 = PA11;
396 IO_4 = V1.IO_4;
397
398 // public final static int A1_A = PA0;
399 A1_A = V1.A1_A;
400 // public final static int A1_B = PA1;
401 A1_B = V1.A1_B;
402 // public final static int A2_A = PA2;
403 A2_A = V1.A2_A;
404 // public final static int A2_2 = PA3;
405 A2_B = V1.A2_B;
406 }
407
408 // }
409
411 public final static int k_i2cDefAddr = 5;
412
413 public enum ProcessorType {
417 Unknown,
418
422 Arduino,
423
427 STM32
428 }
429
430 public interface Callback {
431 public void callback(ByteBuffer buffer, Object object);
432 }
433
434 public enum RequestType {
435 EncoderRelativePosition,
436 Gyro,
437 }
438
439 private class GroupRequest {
440 private final RequestType m_type;
441 private final int m_id; // Control ID
442 private final GroupRequestCallback m_callback;
443 private final Object m_control;
444
445 public GroupRequest(RequestType type, int id, GroupRequestCallback callback, Object control) {
446 m_type = type;
447 m_id = id;
448 m_callback = callback;
449 m_control = control;
450 }
451 }
452
453 interface GroupRequestCallback {
454 void callback(ByteBuffer data, Object object);
455 }
456
457 class Processor {
458 private int m_addr;
459 private int m_nextEncoder = 0;
460 private long m_validPins = 0;
461 private int m_validAnalogPins = 0;
462
463 Processor(int addr) {
464 m_addr = addr;
465 }
466
467 int getNextEncoderNo() {
468 return m_nextEncoder++;
469 }
470
471 private long getValidPins() {
472 if (m_validPins == 0) {
473 m_validPins = m_device.getLong(m_addr, TwoWire.k_getValidOutputPins);
474 }
475
476 return (m_validPins);
477 }
478
479 private int getValidAnalogPins() {
480 if (m_validAnalogPins == 0) {
481 m_validAnalogPins = m_device.getInt(m_addr, TwoWire.k_getValidAnalogPins);
482 }
483
484 System.out.println(String.format("validAnalogPins = %x", m_validAnalogPins));
485
486 return (m_validAnalogPins);
487 }
488
489 void verifyPin(int pin, boolean analog) {
490 long validPins = analog ? getValidAnalogPins() : getValidPins();
491
492 /*
493 * Check if pin is valid for this processor
494 */
495 if ((pin < 0) || (pin >= 64) || (((1 << pin) & validPins) == 0)) {
496 throw new TwoWire.HardwareException(
497 String.format("Encoder: Invalid%s pin: %d", analog ? " Analog" : "", pin));
498 }
499
500 /*
501 * Check if pin is currently in use
502 */
503 if (m_device.checkUsedPin(m_addr, pin)) {
504 throw new TwoWire.HardwareException(String.format("Encoder: Pin %d is in use", pin));
505 }
506 }
507
508 // private boolean m_runCommandAdded = false;
509
510 // public void addRunCommand() {
511 // if (!m_runCommandAdded) {
512 // Logger.log("Encoder", 1, "Adding run command");
513
514 // m_device.addRunCommand(()->readAllPositions());
515 // m_runCommandAdded = true;
516 // }
517 // }
518
519 private List<GroupRequest> m_groupRequests = Collections.synchronizedList(new ArrayList<GroupRequest>());
520
521 private boolean m_groupRequestsStarted = false;
522
523 public void addGroupRequest(RequestType type, int id, GroupRequestCallback callback, Object control) {
524 synchronized(m_groupRequests) {
525 m_groupRequests.add(new GroupRequest(type, id, callback, control));
526 }
527 }
528
529 private void groupRequestCallback(ByteBuffer data, Object object) {
530 synchronized(m_groupRequests) {
531 for (GroupRequest request : m_groupRequests) {
532 switch (request.m_type) {
533 case EncoderRelativePosition:
534 if (data.remaining() < 2) {
535 throw new HardwareException("Invalid group request data for encoder");
536 }
537 request.m_callback.callback(data, request.m_control);
538 break;
539
540 case Gyro:
541 if (data.remaining() < 3) {
542 throw new HardwareException("Invalid group request data for gyro");
543 }
544 request.m_callback.callback(data, request.m_control);
545 break;
546 }
547 }
548 }
549 }
550
551 private void runGroupRequests() {
552 synchronized (m_groupRequests) {
553 byte[] data = new byte[m_groupRequests.size()];
554
555 int i = 0;
556 int responseSize = 0;
557 for (GroupRequest request : m_groupRequests) {
558 switch (request.m_type) {
559 case EncoderRelativePosition:
560 responseSize++;
561 break;
562
563 case Gyro:
564 // Logger.log("Device", 1, "Gyro group");
565 responseSize += 2;
566 break;
567 }
568 data[i++] = (byte) (request.m_id | (request.m_type.ordinal() << 4));
569 }
570
571 m_device.getBytes(m_addr, TwoWire.k_getMultipleEncoderPos, data, responseSize,
572 (b, o) -> groupRequestCallback(b, o));
573 }
574 }
575
576 public void startGroupRequest() {
577 // Logger.log("Device.Processor", 1, "startGroupRequest");
578
579 if (!m_groupRequestsStarted) {
580 m_groupRequestsStarted = true;
581 m_twoWire.addRunCommand(() -> runGroupRequests());
582 }
583 }
584
585 }
586
587 static List<Processor> m_processors = Collections.synchronizedList(new ArrayList<Processor>());
588
589 public static void startGroupRequests() {
590 // Logger.log("Device", 1, "startGroupRequests");
591
592 for (Processor processor : m_processors) {
593 processor.startGroupRequest();
594 }
595 }
596
597 Processor getProcessor(int addr) {
598 for (Processor processor : m_processors) {
599 if (processor.m_addr == addr) {
600 return (processor);
601 }
602 }
603
604 Processor processor = new Processor(addr);
605 m_processors.add(processor);
606
607 return (processor);
608 }
609
610 // public class Processorx {
611 // int m_addr;
612 // private List<GroupRequest> m_groupRequests = Collections.synchronizedList(new
613 // ArrayList<GroupRequest>());
614
615 // private boolean m_groupRequestsStarted = false;
616
617 // public void addGroupRequest(RequestType type, int id, Object control) {
618 // m_groupRequests.add(new GroupRequest(type, id, control));
619 // }
620
621 // private void groupRequestCallback(ByteBuffer data) {
622
623 // }
624
625 // private void runGroupRequests() {
626 // byte[] data = new byte[m_groupRequests.size()];
627
628 // int i = 0;
629 // for (GroupRequest request : m_groupRequests) {
630 // data[i++] = (byte) request.m_id; // TODO - need to handle all types
631 // }
632
633 // m_device.getBytes(m_addr, TwoWire.k_getMultipleEncoderPos, data, (b) ->
634 // groupRequestCallback(b));
635 // }
636
637 // public void startGroupRequests() {
638 // if (!m_groupRequestsStarted) {
639 // m_twoWire.addRunCommand(() -> runGroupRequests());
640 // }
641 // }
642 // }
643
644 // private List<Processor> m_processors = Collections.synchronizedList(new
645 // ArrayList<Processor>());
646
647 public static int m_boardVersion = 0;
648 public static int m_boardSubVersion = 0;
649 // private Serial m_serial;
650 private SerialConnection m_serialConnection;
651 private static Device m_device;
652 private String m_command = "";
653
654 @SuppressWarnings("unused")
655 private int m_maxAnalogInputs = 0;
656 // private int m_maxDigitalInputs = 0;
657 // private int m_maxMotors = 0;
658 // private int m_maxAnalogEncoders = 0;
659 // private int m_maxQuadEncoders = 0;
660 // private int m_maxLEDStrings = 0;
661 // private int m_maxMotionProfile = 0;
662
663 @SuppressWarnings("unused")
664 private int m_pins = 0;
665 // private Timer m_pingTimer = new Timer();
666 // private int[] m_analog = new int[8];
667
668
669 // private final GpioPinDigitalOutput m_trigPin;
670
671 /*
672 * TwoWire
673 */
674 public final static int k_mainTwoWireId = 5; // ID of main STM32
675 // public final static int k_processorArduino = 1;
676 // public final static int k_processorSTM32 = 2;
677 private final TwoWire m_twoWire = new TwoWire();
678 // private int m_validPins = 0;
679
680 /*
681 * Requests
682 */
683 // private final int k_requestProcessorType = 129;
684
685 public enum PinMode {
686 Input,
687 Output
688 };
689
690 GPIOInterface m_gpio;
691
692 private Device() {
693 m_gpio = new GPIOInterface();
694
695 // if (!PCTest) {
696 // m_gpio = GpioFactory.getInstance();
697 // m_resetPin = m_gpio.provisionDigitalOutputPin(RaspiPin.GPIO_01, "Reset", PinState.HIGH);
698 // m_resetPin.setMode(com.pi4j.io.gpio.PinMode.DIGITAL_OUTPUT);
699 // m_resetPin.high();
700 // // System.out.println("Set GPI01 high");
701 // // m_trigPin = m_gpio.provisionDigitalOutputPin(RaspiPin.GPIO_00, "Trigger",
702 // // PinState.HIGH);
703 // // m_trigPin.high();
704 // } else {
705 // m_gpio = null;
706 // m_resetPin = null;
707 // // m_trigPin = null;
708 // }
709 }
710
711 public void addRunCommand(RunCommand command) {
712 m_twoWire.addRunCommand(command);
713 }
714
719 public int getBoardVersion(int addr) {
720 int fullVersion = m_twoWire.sendRequestByte(addr, TwoWire.k_getBoardVersion);
721 int version = fullVersion & 0xf;
722
723 Logger.log("Device", 1, String.format("getBoardVersion=%d", version));
724
725
726 if (version == 1) {
727 Device.setVersion1();
728 }
729 else if (version == 3) {
730 Swerve.setV3();
731 }
732 else if (version == 4) {
733 Minibot.setV4();
734 }
735 else if (version > 4) {
736 throw new HardwareException(String.format("Unknown board type: %d", version));
737 }
738
739 return fullVersion;
740 }
741
742 public int getBoardVersion() {
743 return (getBoardVersion(k_mainTwoWireId));
744 }
745
746 // public int getValidPins(int addr)
747 // {
748 // return(m_twoWire.sendRequestInt(addr, TwoWire.k_getValidPins));
749 // }
750
751 // public int getValidPWMPins(int addr)
752 // {
753 // return(m_twoWire.sendRequestInt(addr, TwoWire.k_getValidPWMPins));
754 // }
755
756 // Takes n single byte arguments and return n bytes of data
757 public void getBytes(int addr, int command, byte args[], int responseSize, Callback callback) {
758 m_twoWire.sendRequestBytes(addr, command, args, responseSize, callback);
759 }
760
761 public long getLong(int addr, int command) {
762 return (m_twoWire.sendRequestLong(addr, command));
763 }
764
765 public int getInt(int addr, int command) {
766 return (m_twoWire.sendRequestInt(addr, command));
767 }
768
769 public int getInt(int addr, int command, byte byte1) {
770 return (m_twoWire.sendRequestInt(addr, command, byte1));
771 }
772
773 public void getInt(int addr, int command, byte byte1, Callback callback, Object object) {
774 m_twoWire.sendRequestInt(addr, command, byte1, callback, object);
775 }
776
777 public int getShort(int addr, int command, byte byte1) {
778 return (m_twoWire.sendRequestShort(addr, command, byte1));
779 }
780
781 public int getByte(int addr, int command) {
782 return (m_twoWire.sendRequestByte(addr, command));
783 }
784
785 public int getByte(int addr, int command, byte byte1) {
786 return (m_twoWire.sendRequestByte(addr, command, byte1));
787 }
788
789 public ByteBuffer getData(int addr, int command, int size) {
790 return (m_twoWire.sendRequest(addr, command, size));
791 }
792
793 public boolean checkUsedPin(int addr, int pin) {
794 return (m_twoWire.checkUsedPin(addr, pin));
795 }
796
797 public boolean isPinValid(int addr, int pin, PinType type) {
798 return m_twoWire.isPinValid(addr, pin, type);
799 }
800
801 public void setUsedPin(int addr, int pin) {
802 m_twoWire.setUsedPin(addr, pin);
803 }
804
805 public int getEncoderRange(int addr) {
806 return m_twoWire.getEncoderRange(addr);
807 }
808
809 private void waitForReset() {
810 Logger.log("RobotBase", 0, "Waiting for response");
811
812 RobotCore.sleep(3000);
813
814 // int pins = m_twoWire.sendRequestInt(5, TwoWire.k_getValidPWMPins);
815 // Logger.log("Device", 1, String.format("pins=0x%x", pins));
816 // while (true);
817
818 m_boardVersion = getBoardVersion();
819 m_boardSubVersion = m_boardVersion >> 4;
820 m_boardVersion &= 0xf;
821 Logger.log("RobotBase", 0, String.format("type=%d,version=%d", m_boardVersion, m_boardSubVersion));
822 }
823
824 private void resetDevice() {
825 Logger.log("RobotBase", 2, "ResetArduino");
826
827 if (!PCTest) {
828 // m_resetPin.low();
829 m_gpio.setReset(false);
830 RobotCore.sleep(10);
831 m_gpio.setReset(true);
832 // m_resetPin.high();
833
834 waitForReset();
835 }
836 }
837
838 // public void trigger() {
839 // m_trigPin.low();
840 // RobotCoreBase.sleep(1);
841 // m_trigPin.high();
842 // }
843
844 public static Device getInstance() {
845 if (m_device == null) {
846 m_device = new Device();
847
848 try {
849 m_device.start();
850 // m_device.SetTimeout(1500);
851 } catch (InterruptedException ex) {
852
853 }
854 }
855
856 return (m_device);
857 }
858
859 private void dataReceived(String data) {
860 int idx;
861
862 // System.out.println("DataReceived: " + data);
863
864 while ((idx = data.indexOf('\n')) >= 0) {
865 m_command += data.substring(0, idx);
866 // ProcessCommand(m_command);
867 Logger.log("Device", 1, String.format("STM:%s", m_command));
868
869 m_command = "";
870
871 data = data.substring(idx + 1);
872 }
873
874 m_command += data;
875 }
876
878 public void start() throws InterruptedException {
879 Logger.log("Device", 1, "Start receiver");
880
881 try {
882 if (!PCTest) {
883 if (m_serialConnection != null) {
884 m_serialConnection.open((d) -> {
885 dataReceived(d);
886 });
887 }
888 }
889 } catch (Exception ex) {
890 System.out.println(" ==>> SERIAL SETUP FAILED : " + ex.getMessage());
891 return;
892 }
893
894 resetDevice();
895
896 /*
897 * Start keep-alive thread
898 */
899 new Thread(new Runnable() {
900 @Override
901 public void run() {
902 while (true) {
903 // System.out.println("Sending keep alive");
904 sendCommandToAll(TwoWire.k_cmdKeepAlive);
905
906 try {
907 Thread.sleep(500);
908 } catch (InterruptedException ex) {
909
910 }
911 }
912 }
913
914 }).start();
915 }
917
918 public void sendCommandToAll(int command) {
919 m_twoWire.sendCommandToAll(command);
920 }
921
922 public void sendCommand(int addr, int command, byte[] bytes) {
923 m_twoWire.sendCommand(addr, command, bytes);
924 }
925
926 public void sendCommand(int addr, int command, byte byte1, byte byte2, byte byte3, byte byte4) {
927 m_twoWire.sendCommand(addr, command, byte1, byte2, byte3, byte4);
928 }
929
930 public void sendCommand(int addr, int command, byte byte1, short value) {
931 m_twoWire.sendCommand(addr, command, byte1, value);
932 }
933
934 public void sendCommand(int addr, int command, byte byte1, int value) {
935 m_twoWire.sendCommand(addr, command, byte1, value);
936 }
937
938 public void sendCommand(int addr, int command, byte byte1, float value) {
939 m_twoWire.sendCommand(addr, command, byte1, value);
940 }
941
942 public void sendCommand(int addr, int command, byte byte1, byte byte2) {
943 m_twoWire.sendCommand(addr, command, byte1, byte2);
944 }
945
946 public void sendCommand(int addr, int command, int yaw, int x, int y) {
947 m_twoWire.sendCommand(addr, command, yaw, x, y);
948 }
949
950 public void sendCommand(int addr, int command, byte byte1) {
951 m_twoWire.sendCommand(addr, command, byte1);
952 }
953
954 public void sendCommand(int addr, int command) {
955 m_twoWire.sendCommand(addr, command);
956 }
958
959}
robotCore.Device.IO_4
static int IO_4
Definition Device.java:371
robotCore.Device.A2_B
static int A2_B
Definition Device.java:307
robotCore.Device.Q3_INT
static final int Q3_INT
Definition Device.java:329
robotCore.Device.IO_2
static int IO_2
Definition Device.java:361
robotCore.Device.M2_2_PWM
static final int M2_2_PWM
Definition Device.java:272
robotCore.Device.M3_2_PWM
static final int M3_2_PWM
Definition Device.java:288
robotCore.Device.Q1_INT
static final int Q1_INT
Definition Device.java:312
robotCore.Device.Q4_INT
static int Q4_INT
Definition Device.java:338
robotCore.Device.M1_1_PWM
static final int M1_1_PWM
Definition Device.java:247
robotCore.Device.Q2_DIR
static final int Q2_DIR
Definition Device.java:324
robotCore.Device.Q2_INT
static final int Q2_INT
Definition Device.java:320
robotCore.Device.M1_2_DIR
static int M1_2_DIR
Definition Device.java:252
robotCore.Device.Q5_INT
static final int Q5_INT
Definition Device.java:347
robotCore.Device.M2_1_PWM
static final int M2_1_PWM
Definition Device.java:264
robotCore.Device.A1_A
static int A1_A
Definition Device.java:293
robotCore.Device.A1_B
static int A1_B
Definition Device.java:297
robotCore.Device.Q1_DIR
static final int Q1_DIR
Definition Device.java:316
robotCore.Device.M1_2_PWM
static final int M1_2_PWM
Definition Device.java:256
robotCore.Device.Q4_DIR
static int Q4_DIR
Definition Device.java:342
robotCore.Device.Q3_DIR
static final int Q3_DIR
Definition Device.java:333
robotCore.Device.IO_1
static int IO_1
Definition Device.java:357
robotCore.Device.M2_2_DIR
static final int M2_2_DIR
Definition Device.java:268
robotCore.Device.M3_2_DIR
static int M3_2_DIR
Definition Device.java:284
robotCore.Device.M3_1_DIR
static final int M3_1_DIR
Definition Device.java:276
robotCore.Device.M2_1_DIR
static final int M2_1_DIR
Definition Device.java:260
robotCore.Device.Q5_DIR
static int Q5_DIR
Definition Device.java:352
robotCore.Device.M3_1_PWM
static final int M3_1_PWM
Definition Device.java:280
robotCore.Device.A2_A
static int A2_A
Definition Device.java:302
robotCore.Device.M1_1_DIR
static int M1_1_DIR
Definition Device.java:243
robotCore.Device.IO_3
static int IO_3
Definition Device.java:366
robotCore.Logger
The Logger class provides a mechanism to log messages.
Definition Logger.java:54
robotCore.Logger.log
static void log(String tag, int level, String message, boolean noLogToConsole)
Definition Logger.java:161