mirror of
https://github.com/gdsports/USBHost_t36
synced 2024-11-27 19:42:15 -05:00
4eee8627d2
As per the forum posts, worked on combining the claim code for extracting which endpoints are RX and TX and create the pipes and the like. Also worked on live cases. Reading and writing to AX servos... Found some issues Probably more to find. I left in some debug code to help find cases of deleting a pipe may hang. Also macros in serial.cpp to set or toggle IO pins (when in debug mode). This helps find where things are happing using Logic Analyzer as a way to find the approriate USB packets.
318 lines
8.9 KiB
C++
318 lines
8.9 KiB
C++
// Simple test of USB Host Mouse/Keyboard
|
|
//
|
|
// This example is in the public domain
|
|
|
|
#include "USBHost_t36.h"
|
|
#define USBBAUD 115200
|
|
uint32_t baud = USBBAUD;
|
|
uint32_t format = USBHOST_SERIAL_8N1;
|
|
USBHost myusb;
|
|
USBHub hub1(myusb);
|
|
USBHub hub2(myusb);
|
|
USBHIDParser hid1(myusb);
|
|
USBHIDParser hid2(myusb);
|
|
USBHIDParser hid3(myusb);
|
|
USBSerial userial(myusb);
|
|
|
|
USBDriver *drivers[] = {&hub1, &hub2, &hid1, &hid2, &hid3, &userial};
|
|
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
|
|
const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "HID1", "HID2", "HID3", "USERIAL1" };
|
|
bool driver_active[CNT_DEVICES] = {false, false, false, false};
|
|
|
|
void setup()
|
|
{
|
|
pinMode(13, OUTPUT);
|
|
pinMode(2, OUTPUT);
|
|
pinMode(3, OUTPUT);
|
|
for (int i = 0; i < 5; i++) {
|
|
digitalWrite(2, HIGH);
|
|
delayMicroseconds(50);
|
|
digitalWrite(2, LOW);
|
|
delayMicroseconds(50);
|
|
}
|
|
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor
|
|
Serial.println("\n\nUSB Host Testing - Serial");
|
|
myusb.begin();
|
|
Serial1.begin(115200); // We will echo stuff Through Serial1...
|
|
|
|
}
|
|
|
|
|
|
void loop()
|
|
{
|
|
digitalWrite(13, !digitalRead(13));
|
|
myusb.Task();
|
|
// Print out information about different devices.
|
|
for (uint8_t i = 0; i < CNT_DEVICES; i++) {
|
|
if (*drivers[i] != driver_active[i]) {
|
|
if (driver_active[i]) {
|
|
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
|
|
driver_active[i] = false;
|
|
} else {
|
|
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
|
|
driver_active[i] = true;
|
|
|
|
const uint8_t *psz = drivers[i]->manufacturer();
|
|
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
|
|
psz = drivers[i]->product();
|
|
if (psz && *psz) Serial.printf(" product: %s\n", psz);
|
|
psz = drivers[i]->serialNumber();
|
|
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
|
|
|
|
// If this is a new Serial device.
|
|
if (drivers[i] == &userial) {
|
|
// Lets try first outputting something to our USerial to see if it will go out...
|
|
userial.begin(baud);
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (Serial.available()) {
|
|
Serial.println("Serial Available");
|
|
while (Serial.available()) {
|
|
int ch = Serial.read();
|
|
if (ch == '$') {
|
|
BioloidTest();
|
|
while (Serial.read() != -1);
|
|
} else if (ch == '#') {
|
|
// Lets see if we have a baud rate specified here...
|
|
uint32_t new_baud = 0;
|
|
for(;;) {
|
|
ch = Serial.read();
|
|
if ((ch < '0') || (ch > '9'))
|
|
break;
|
|
new_baud = new_baud*10 + ch - '0';
|
|
}
|
|
// See if the user is specifying a format: 8n1, 7e1, 7e2, 8n2
|
|
// Note this is Quick and very dirty code...
|
|
//
|
|
if (ch == ',') {
|
|
char command_line[10];
|
|
ch = Serial.read();
|
|
while (ch == ' ') Serial.read(); // ignore any spaces.
|
|
uint8_t cb = 0;
|
|
while ((ch > ' ') && (cb < sizeof(command_line))) {
|
|
command_line[cb++] = ch;
|
|
ch = Serial.read();
|
|
}
|
|
command_line[cb] = '\0';
|
|
if (CompareStrings(command_line, "8N1")) format = USBHOST_SERIAL_8N1;
|
|
else if (CompareStrings(command_line, "8N2")) format = USBHOST_SERIAL_8N2;
|
|
else if (CompareStrings(command_line, "7E1")) format = USBHOST_SERIAL_7E1;
|
|
else if (CompareStrings(command_line, "7O1")) format = USBHOST_SERIAL_7O1;
|
|
}
|
|
Serial.println("\n*** Set new Baud command ***\n do userial.end()");
|
|
digitalWriteFast(2, HIGH);
|
|
userial.end(); // Do the end statement;
|
|
digitalWriteFast(2, LOW);
|
|
if (new_baud) {
|
|
baud = new_baud;
|
|
Serial.print(" New Baud: ");
|
|
Serial.println(baud);
|
|
Serial.print(" Format: ");
|
|
Serial.println(format, HEX);
|
|
digitalWriteFast(3, HIGH);
|
|
userial.begin(baud, format);
|
|
digitalWriteFast(3, LOW);
|
|
Serial.println(" Completed ");
|
|
} else {
|
|
Serial.println(" New Baud 0 - leave disabled");
|
|
}
|
|
|
|
while (Serial.read() != -1);
|
|
} else {
|
|
userial.write(ch);
|
|
}
|
|
}
|
|
}
|
|
|
|
while (Serial1.available()) {
|
|
// Serial.println("Serial1 Available");
|
|
Serial1.write(Serial1.read());
|
|
}
|
|
|
|
while (userial.available()) {
|
|
// Serial.println("USerial Available");
|
|
|
|
Serial.write(userial.read());
|
|
}
|
|
}
|
|
|
|
bool CompareStrings(const char *sz1, const char *sz2) {
|
|
while (*sz2 != 0) {
|
|
if (toupper(*sz1) != toupper(*sz2))
|
|
return false;
|
|
sz1++;
|
|
sz2++;
|
|
}
|
|
return true; // end of string so show as match
|
|
|
|
|
|
}
|
|
|
|
//#define ID_MASTER 200
|
|
#define ID_MASTER 0xfd
|
|
// Extract stuff from Bioloid library..
|
|
#define AX12_BUFFER_SIZE 128
|
|
#define COUNTER_TIMEOUT 12000
|
|
|
|
/** Instruction Set **/ #define AX_PING 1
|
|
#define AX_READ_DATA 2
|
|
#define AX_WRITE_DATA 3
|
|
#define AX_REG_WRITE 4
|
|
#define AX_ACTION 5
|
|
#define AX_RESET 6
|
|
#define AX_SYNC_WRITE 131
|
|
|
|
#define AX_TORQUE_ENABLE 24
|
|
#define AX_LED 25
|
|
#define AX_CW_COMPLIANCE_MARGIN 26
|
|
#define AX_CCW_COMPLIANCE_MARGIN 27
|
|
#define AX_CW_COMPLIANCE_SLOPE 28
|
|
#define AX_CCW_COMPLIANCE_SLOPE 29
|
|
#define AX_GOAL_POSITION_L 30
|
|
#define AX_GOAL_POSITION_H 31
|
|
#define AX_GOAL_SPEED_L 32
|
|
#define AX_GOAL_SPEED_H 33
|
|
#define AX_TORQUE_LIMIT_L 34
|
|
#define AX_TORQUE_LIMIT_H 35
|
|
#define AX_PRESENT_POSITION_L 36
|
|
#define AX_PRESENT_POSITION_H 37
|
|
|
|
|
|
void BioloidTest() {
|
|
uint8_t master_id = 200;
|
|
Serial.println("\n*** Bioloid Test ***");
|
|
if (ax12GetRegister(master_id, 0, 1) != -1) {
|
|
Serial.println("Controller found at 200");
|
|
} else {
|
|
Serial.println("Controller not at 200 try 0xfd");
|
|
master_id = 0xfd;
|
|
if (ax12GetRegister(master_id, 0, 1) != -1) {
|
|
Serial.println("Controller found at 0xfd");
|
|
} else {
|
|
Serial.println("Controller not found");
|
|
}
|
|
}
|
|
for (uint8_t reg = 0; reg < 10; reg++) {
|
|
myusb.Task();
|
|
|
|
Serial.print(ax12GetRegister(master_id, reg, 1), HEX);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.println();
|
|
// Now assuming we found controller...
|
|
// May need to turn on power on controller
|
|
ax12SetRegister(master_id, AX_TORQUE_ENABLE, 1);
|
|
delay(2);
|
|
|
|
// Lets see if we can get the current position for any servo
|
|
for (int i = 0; i < 254; i++) {
|
|
int servo_pos = ax12GetRegister(i, AX_PRESENT_POSITION_L, 2);
|
|
if (servo_pos != -1) {
|
|
Serial.printf("Servo: %d Pos: %d\n", i, servo_pos);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
unsigned char ax_rx_buffer[AX12_BUFFER_SIZE];
|
|
int ax12GetRegister(int id, int regstart, int length) {
|
|
// 0xFF 0xFF ID LENGTH INSTRUCTION PARAM... CHECKSUM
|
|
int return_value;
|
|
digitalWriteFast(2, HIGH);
|
|
int checksum = ~((id + 6 + regstart + length) % 256);
|
|
userial.write(0xFF);
|
|
userial.write(0xFF);
|
|
userial.write(id);
|
|
userial.write(4); // length
|
|
userial.write(AX_READ_DATA);
|
|
userial.write(regstart);
|
|
userial.write(length);
|
|
userial.write(checksum);
|
|
userial.flush(); // make sure the data goes out.
|
|
|
|
if (ax12ReadPacket(length + 6) > 0) {
|
|
// ax12Error = ax_rx_buffer[4];
|
|
if (length == 1)
|
|
return_value = ax_rx_buffer[5];
|
|
else
|
|
return_value = ax_rx_buffer[5] + (ax_rx_buffer[6] << 8);
|
|
} else {
|
|
digitalWriteFast(3, !digitalReadFast(3));
|
|
return_value = -1;
|
|
}
|
|
digitalWriteFast(2, LOW);
|
|
return return_value;
|
|
|
|
}
|
|
|
|
void ax12SetRegister(int id, int regstart, int data){
|
|
int checksum = ~((id + 4 + AX_WRITE_DATA + regstart + (data&0xff)) % 256);
|
|
userial.write(0xFF);
|
|
userial.write(0xFF);
|
|
userial.write(id);
|
|
userial.write(4); // length
|
|
userial.write(AX_WRITE_DATA);
|
|
userial.write(regstart);
|
|
userial.write(data&0xff);
|
|
// checksum =
|
|
userial.write(checksum);
|
|
userial.flush();
|
|
//ax12ReadPacket();
|
|
}
|
|
|
|
|
|
|
|
int ax12ReadPacket(int length) {
|
|
unsigned long ulCounter;
|
|
unsigned char offset, checksum;
|
|
unsigned char *psz;
|
|
unsigned char *pszEnd;
|
|
int ch;
|
|
|
|
|
|
offset = 0;
|
|
|
|
psz = ax_rx_buffer;
|
|
pszEnd = &ax_rx_buffer[length];
|
|
|
|
while (userial.read() != -1) ;
|
|
uint32_t ulStart = millis();
|
|
// Need to wait for a character or a timeout...
|
|
do {
|
|
ulCounter = COUNTER_TIMEOUT;
|
|
while ((ch = userial.read()) == -1) {
|
|
if ((millis() - ulStart) > 10) {
|
|
//if (!--ulCounter) {
|
|
Serial.println("Timeout");
|
|
return 0; // Timeout
|
|
}
|
|
}
|
|
} while (ch != 0xff) ;
|
|
*psz++ = 0xff;
|
|
while (psz != pszEnd) {
|
|
ulCounter = COUNTER_TIMEOUT;
|
|
while ((ch = userial.read()) == -1) {
|
|
//Serial.printf("Read ch: %x\n", ch);
|
|
if (!--ulCounter) {
|
|
return 0; // Timeout
|
|
}
|
|
}
|
|
*psz++ = (unsigned char)ch;
|
|
}
|
|
checksum = 0;
|
|
for (offset = 2; offset < length; offset++)
|
|
checksum += ax_rx_buffer[offset];
|
|
if (checksum != 255) {
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|