Complete the code for the driver's station Teensy
Change-Id: I0f1a365263a7eec2d355610c6722b3cd29dd0430
diff --git a/motors/medium_salsa.cc b/motors/medium_salsa.cc
index 2a9804a..3ac0337 100644
--- a/motors/medium_salsa.cc
+++ b/motors/medium_salsa.cc
@@ -178,7 +178,7 @@
AdcInitMedium();
MathInit();
delay(1000);
- can_init();
+ can_init(0, 1);
GPIOD_PCOR = 1 << 3;
GPIO_BITBAND(GPIOD_PDDR, 3) = 1;
@@ -243,7 +243,7 @@
while (true) {
unsigned char command_data[8];
int command_length;
- can_receive_command(command_data, &command_length);
+ can_receive_command(command_data, &command_length, 0);
if (command_length == 4) {
uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
command_data[2] << 8 | command_data[3];
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index 9db9f8f..62f0398 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -19,7 +19,7 @@
// 16. Using fewer means less for the CAN module (and CPU) to go through looking
// for actual data.
// 0 is for sending and 1 is for receiving commands.
-#define NUMBER_MESSAGE_BUFFERS 2
+#define NUMBER_MESSAGE_BUFFERS 4
#if NUMBER_MESSAGE_BUFFERS > 16
#error Only have 16 message buffers on this part.
@@ -27,7 +27,7 @@
// TODO(Brian): Do something about CAN errors and warnings (enable interrupts?).
-void can_init() {
+void can_init(uint32_t id0, uint32_t id1) {
printf("can_init\n");
SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0;
@@ -53,17 +53,29 @@
// Initialize all the buffers and RX filters we're enabling.
// Just in case this does anything...
- CAN0_RXIMRS[0] = 0;
- CAN0_MESSAGES[0].prio_id = 0;
+ CAN0_RXIMRS[2] = 0;
+ CAN0_MESSAGES[2].prio_id = 0;
+ CAN0_MESSAGES[2].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE);
+
+ CAN0_RXIMRS[3] = 0;
+ CAN0_MESSAGES[3].prio_id = 0;
+ CAN0_MESSAGES[3].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE);
+
+ CAN0_RXIMRS[0] = (1 << 31) /* Want to filter out RTRs. */ |
+ (0 << 30) /* Want to only get standard frames. */ |
+ (0x1FFC0000) /* Filter on the id. */;
+ CAN0_MESSAGES[0].prio_id = id0 << 18;
CAN0_MESSAGES[0].control_timestamp =
- CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE) | CAN_MB_CONTROL_IDE;
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
CAN0_RXIMRS[1] = (1 << 31) /* Want to filter out RTRs. */ |
- (1 << 30) /* Want to only get extended frames. */ |
- 0xFF /* Filter on the 1-byte VESC id. */;
- CAN0_MESSAGES[1].prio_id = 0;
+ (0 << 30) /* Want to only get standard frames. */ |
+ (0x1FFC0000) /* Filter on the id. */;
+ CAN0_MESSAGES[1].prio_id = id1 << 18;
CAN0_MESSAGES[1].control_timestamp =
- CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY) | CAN_MB_CONTROL_IDE;
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
// Using the oscillator clock directly because it's a reasonable frequency and
// more stable than the PLL-based peripheral clock, which matters.
@@ -165,7 +177,7 @@
return 0;
}
-void can_receive_command(unsigned char *data, int *length) {
+void can_receive_command(unsigned char *data, int *length, int mailbox) {
if (0) {
static int i = 0;
if (i++ == 10000) {
@@ -174,9 +186,9 @@
i = 0;
}
}
- if ((CAN0_IFLAG1 & (1 << 1)) == 0) {
+ if ((CAN0_IFLAG1 & (1 << mailbox)) == 0) {
*length = -1;
return;
}
- can_vesc_process_rx(&CAN0_MESSAGES[1], data, length);
+ can_vesc_process_rx(&CAN0_MESSAGES[mailbox], data, length);
}
diff --git a/motors/peripheral/can.h b/motors/peripheral/can.h
index 1bdf6e8..54bdce8 100644
--- a/motors/peripheral/can.h
+++ b/motors/peripheral/can.h
@@ -10,14 +10,15 @@
extern "C" {
#endif
-void can_init(void);
+void can_init(uint32_t id0, uint32_t id1);
// Mailbox is 2 or 3 for the two send mailboxes.
int can_send(uint32_t can_id, const unsigned char *data, unsigned int length,
unsigned int mailbox);
// Sets *length to -1 if there isn't a new piece of data to receive.
-void can_receive_command(unsigned char *data, int *length);
+// Mailbox is 0 or 1 for the two receive mailboxes.
+void can_receive_command(unsigned char *data, int *length, int mailbox);
#ifdef __cplusplus
}
diff --git a/motors/pistol_grip/drivers_station.cc b/motors/pistol_grip/drivers_station.cc
index 4c68afc..d58d3d3 100644
--- a/motors/pistol_grip/drivers_station.cc
+++ b/motors/pistol_grip/drivers_station.cc
@@ -99,56 +99,101 @@
}
}
-void MoveJoysticks(teensy::HidFunction *joysticks,
- teensy::InterruptOut *interrupt_out) {
- static uint8_t x_axis = 0, y_axis = 97, z_axis = 5, rz_axis = 8;
- while (true) {
- {
- char buffer[64];
- if (interrupt_out->ReceiveData(buffer) > 0) {
- if (buffer[0]) {
- GPIOC_PCOR = 1 << 5;
- } else {
- GPIOC_PSOR = 1 << 5;
- }
- }
-
- DisableInterrupts disable_interrupts;
- uint8_t buttons = 0;
- if (x_axis % 8u) {
- buttons = 2;
- }
- uint8_t report[10] = {x_axis, 0, y_axis, 0, z_axis,
- rz_axis, 0, 0, buttons, 0};
- joysticks->UpdateReport(report, 10, disable_interrupts);
- }
- delay(1);
- x_axis += 1;
- y_axis += 3;
- z_axis += 5;
- rz_axis += 8;
- }
-}
-
-void ForwardJoystickData(teensy::HidFunction *joysticks) {
+void ForwardJoystickData(teensy::HidFunction *throttle_joystick,
+ teensy::HidFunction *wheel_joystick,
+ teensy::InterruptOut *interrupt_out) {
uint32_t last_command_time = micros();
+ uint16_t trigger_position = 0x8000;
+ uint16_t wheel_position = 0x8000;
+ uint16_t trigger_velocity = 0x8000;
+ uint16_t wheel_velocity = 0x8000;
+ uint16_t trigger_torque = 0x8000;
+ uint16_t wheel_torque = 0x8000;
+ uint16_t buttons = 0x0080;
while (true) {
- uint8_t data[8];
+ bool update_report = false;
+
+ uint8_t can_data[8];
int length;
- can_receive_command(data, &length);
- if (length == 3) {
+ can_receive_command(can_data, &length, 0);
+ if (length == 8) {
last_command_time = micros();
- DisableInterrupts disable_interrupts;
- joysticks->UpdateReport(data, 3, disable_interrupts);
+ trigger_position =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[0]) |
+ (static_cast<uint32_t>(can_data[1]) << 8));
+ trigger_velocity =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[2]) |
+ (static_cast<uint32_t>(can_data[3]) << 8));
+ trigger_torque =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[4]) |
+ (static_cast<uint32_t>(can_data[5]) << 8));
+
+ buttons = static_cast<uint16_t>(
+ (buttons & 0xc) | (static_cast<uint32_t>(can_data[7] & 0xc0) >> 6));
+ update_report = true;
+ }
+
+ can_receive_command(can_data, &length, 1);
+ if (length == 8) {
+ last_command_time = micros();
+ wheel_position =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[0]) |
+ (static_cast<uint32_t>(can_data[1]) << 8));
+ wheel_velocity =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[2]) |
+ (static_cast<uint32_t>(can_data[3]) << 8));
+ wheel_torque =
+ static_cast<uint16_t>(static_cast<uint32_t>(can_data[4]) |
+ (static_cast<uint32_t>(can_data[5]) << 8));
+
+ buttons = static_cast<uint16_t>(
+ (buttons & 0x3) | (static_cast<uint32_t>(can_data[7] & 0xc0) >> 4));
+ update_report = true;
}
static constexpr uint32_t kTimeout = 10000;
if (!time_after(time_add(last_command_time, kTimeout), micros())) {
+ trigger_position = 0x8000;
+ wheel_position = 0x8000;
+ trigger_velocity = 0x8000;
+ wheel_velocity = 0x8000;
+ trigger_torque = 0x8000;
+ wheel_torque = 0x8000;
+ buttons = 0x0080;
+ update_report = true;
// Avoid wrapping back into the valid range.
last_command_time = time_subtract(micros(), kTimeout);
- uint8_t zeros[] = {0, 0, 0x80};
+ }
+
+ if (update_report) {
DisableInterrupts disable_interrupts;
- joysticks->UpdateReport(zeros, 3, disable_interrupts);
+
+ const uint16_t trigger_packet[] = {
+ trigger_position,
+ trigger_velocity,
+ trigger_torque,
+ static_cast<uint16_t>((trigger_position & 0xff) << 8),
+ static_cast<uint16_t>((trigger_velocity & 0xff) << 8),
+ static_cast<uint16_t>((trigger_torque & 0xff) << 8),
+ buttons};
+ throttle_joystick->UpdateReport(trigger_packet, 14, disable_interrupts);
+
+ const uint16_t wheel_packet[] = {
+ wheel_position,
+ wheel_velocity,
+ wheel_torque,
+ static_cast<uint16_t>((wheel_position & 0xff) << 8),
+ static_cast<uint16_t>((wheel_velocity & 0xff) << 8),
+ static_cast<uint16_t>((wheel_torque & 0xff) << 8),
+ buttons};
+ wheel_joystick->UpdateReport(wheel_packet, 14, disable_interrupts);
+ }
+
+ char usb_out_data[teensy::InterruptOut::kSize];
+ const int usb_out_size = interrupt_out->ReceiveData(usb_out_data);
+ if (usb_out_size >= 16) {
+ can_send(0x2, reinterpret_cast<unsigned char *>(usb_out_data), 8, 2);
+ can_send(0x3, reinterpret_cast<unsigned char *>(usb_out_data) + 8, 8, 3);
}
}
}
@@ -198,16 +243,17 @@
teensy::UsbDevice usb_device(0, 0x16c0, 0x0491);
usb_device.SetManufacturer("FRC 971 Spartan Robotics");
usb_device.SetProduct("Pistol Grip Controller interface");
+ teensy::HidFunction throttle_joystick(&usb_device, 14);
+ teensy::HidFunction wheel_joystick(&usb_device, 14);
teensy::AcmTty tty1(&usb_device);
teensy::AcmTty tty2(&usb_device);
- teensy::HidFunction joysticks(&usb_device, 10);
teensy::InterruptOut interrupt_out(&usb_device, "JoystickForce");
global_stdout.store(&tty2, ::std::memory_order_release);
usb_device.Initialize();
- can_init();
+ can_init(0, 1);
- MoveJoysticks(&joysticks, &interrupt_out);
+ ForwardJoystickData(&throttle_joystick, &wheel_joystick, &interrupt_out);
return 0;
}
diff --git a/motors/pistol_grip/usb_forward.cc b/motors/pistol_grip/usb_forward.cc
index 830f8f7..19be857 100644
--- a/motors/pistol_grip/usb_forward.cc
+++ b/motors/pistol_grip/usb_forward.cc
@@ -111,11 +111,13 @@
} // namespace
-int main() {
+int main(int argc, char ** /*argv*/) {
CHECK_LIBUSB(libusb_init(nullptr));
libusb_set_debug(nullptr, LIBUSB_LOG_LEVEL_INFO);
- libusb_set_debug(nullptr, LIBUSB_LOG_LEVEL_DEBUG);
+ if (argc > 1) {
+ libusb_set_debug(nullptr, LIBUSB_LOG_LEVEL_DEBUG);
+ }
#ifdef __WINNT__
{
diff --git a/motors/usb/hid.cc b/motors/usb/hid.cc
index 0c5075e..f42930b 100644
--- a/motors/usb/hid.cc
+++ b/motors/usb/hid.cc
@@ -21,7 +21,7 @@
0x09, 0x04, // Usage (Joystick),
0xA1, 0x01, // Collection (Application),
0x75, 0x10, // Report Size (16),
- 0x95, 0x04, // Report Count (4),
+ 0x95, 0x06, // Report Count (6),
0x15, 0x00, // Logical Minimum (0),
0x26, 0xFF, 0xFF, // Logical Maximum (65535),
0x35, 0x00, // Physical Minimum (0),
@@ -29,14 +29,16 @@
0x09, 0x30, // Usage (X),
0x09, 0x31, // Usage (Y),
0x09, 0x32, // Usage (Z),
- 0x09, 0x35, // Usage (Rz),
+ 0x09, 0x33, // Usage (Rz),
+ 0x09, 0x34, // Usage (?),
+ 0x09, 0x35, // Usage (?),
0x81, 0x02, // Input (Variable),
0x75, 0x01, // Report Size (1),
0x95, 0x10, // Report Count (16),
0x25, 0x01, // Logical Maximum (1),
0x45, 0x01, // Physical Maximum (1),
0x05, 0x09, // Usage Page (Button),
- 0x19, 0x01, // Usage Minimum (1),
+ 0x19, 0x01, // Usage Minimum (01),
0x29, 0x10, // Usage Maximum (16),
0x81, 0x02, // Input (Variable),
0xC0 // End Collection
@@ -90,7 +92,7 @@
endpoint_descriptor->AddByte(
m_endpoint_attributes_interrupt()); // bmAttributes
endpoint_descriptor->AddUint16(in_endpoint_max_size()); // wMaxPacketSize
- endpoint_descriptor->AddByte(1); // bInterval
+ endpoint_descriptor->AddByte(0x8); // bInterval
}
}
diff --git a/motors/usb/usb.cc b/motors/usb/usb.cc
index 4a1b115..482d2fd 100644
--- a/motors/usb/usb.cc
+++ b/motors/usb/usb.cc
@@ -130,7 +130,7 @@
device_descriptor_->AddUint16(product_id); // idProduct
// Increment this whenever you need Windows boxes to actually pay attention to
// changes.
- device_descriptor_->AddUint16(7); // bcdDevice
+ device_descriptor_->AddUint16(15); // bcdDevice
// We might overwrite these string descriptor indices later if we get strings
// to put there.
device_descriptor_->AddByte(0); // iManufacturer