void setup() {
Serial.begin(115200);
pinMode(DRIVE_PIN1, OUTPUT);
pinMode(DRIVE_PIN2, OUTPUT);
pinMode(DRIVE_ENABLE_PIN, OUTPUT);
myservo.attach(STEER_SERVO);
myservo.write(DEFAULT_STEER);
Serial.println("Ready");
sCmd.addCommand("T", throttle);
sCmd.addCommand("S", steer);
sCmd.addCommand("R", reset);
sCmd.addCommand("@", ping);
sCmd.addCommand("V", voltage);
sCmd.addCommand("D", distance);
sCmd.addDefaultHandler(unrecognized);
}
void setup()
{
// initialize the serial communication:
serial.init(TTYACM0, 115200, openComport);
serial.setPacketHandler(handlePacket);
sCmd.addCommand(1, respondData);
gettimeofday(&start, NULL);
gettimeofday(&startLoop, NULL);
uiRxPacketCtr=0;
uiTxPacketCtr=0;
}
void CommandSet::setup()
{
/* Setup callbacks for SerialCommand commands */
sCmd.addCommand("L", this->led); // Toggles LED
sCmd.addCommand("ping", this->ping); // Check serial link
sCmd.addCommand("help", this->help);
/* Movement commands */
sCmd.addCommand("M", this->move); // Runs wheel motors
sCmd.addCommand("S", this->stop); // Force stops all motors
sCmd.addCommand("G", this->go); // Runs wheel motors with correction
/* Read from rotary encoders */
sCmd.addCommand("speeds", this->speeds);
/* Misc commands */
sCmd.addCommand("pixels", this->pixels); // Set LED colour
sCmd.addCommand("grab", this->grab); // Grab 0 or 1
sCmd.addCommand("kick", this->kick); // Kick
sCmd.addCommand("shuntkick", this->shuntkick); // Shuntkick
sCmd.addCommand("rotate", this->rotate); // Rotate
/* Debug and inspection commands */
sCmd.addCommand("ps", this->proc_dump); // Show process status
sCmd.addCommand("ptog", this->proc_toggle); // Enable or disable by pid on the fly
sCmd.setDefaultHandler(this->unrecognized); // Handler for command that isn't matched
}
void addCommand(const char *cmd, void(*function)(), const char *wildcard)
{
sCmd.addCommand(cmd, function);
}