// 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;
}
}