// global instance of commander - controller device version
I2CCommanderMaster commander;
void setup() {
// slow start - give RosmoESC a chance to boot up, serial to connect, etc...
delay(1000);
// this is a debug setup so initialize and wait for serial connection
Serial.begin(115200);
while (!Serial);
esp32_setup_peripherals();
ws.onEvent(onWsEvent);
ws_server.addHandler(&ws);
esp32_setup_web_server(&ws_server);
while (!Wire.begin(I2C_SDA, I2C_SCL, 100000ul)) { // standard wire pins
Serial.println("I2C failed to initialize");
delay(1000);
}
commander.addI2CMotors(TARGET_I2C_ADDRESS, 0); // only one motor in my test setup - *Need to add a second motor
commander.addI2CMotors(TARGET_I2C_ADDRESS, 1); // Not sure if I've done this correctly
commander.init();
Serial.println("I2C Commander intialized.");
}
//Original spec:
// messages are always 3 bytes long;
// every message starts with a value 97 - this is a start code of the message. 97 is ASCII 'a' I was just testing it with UART console, that's why I chose this value;
// the second byte represents the right joystick. If it is in the center we send 97 if it is up 98 ('b') and 99 ('c') if it is down;
// the third byte represents the left joystick. If it is in the center we send 97, if it is left 98 ('b') and 99 ('c') if it is right.
// If we loose the websocket connection we command robot to stop.
void setSpeeds(float speed1, float speed2) {
if (commander.writeRegister(0, REG_TARGET, &speed1, 4)!=4) { // 0 is the motor number
Serial.println("Error Motor 0");
}
if (commander.writeRegister(1, REG_TARGET, &speed2, 4)!=4) { // 1 is the motor number