void CommandSet::go()
{
/* holonomics and shit */
float x_vel = atof(sCmd.next());
float y_vel = atof(sCmd.next());
float r_vel = atof(sCmd.next());
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.println(F("Going"));
#endif
_go(x_vel, y_vel, r_vel);
}
void CommandSet::rotate()
{
const int motor_power = atoi(sCmd.next());
const long delta = atoi(sCmd.next());
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.print(F("rotating at "));
Serial.print(motor_power);
Serial.print(F(" to "));
Serial.print(delta);
Serial.println(F(" stops"));
#endif
for (size_t i=0; i < motor_count; i++){
state.motors[i]->power = motor_power;
state.initial_displacement[i] = state.motors[i]->disp;
}
state.rotation_delta = delta;
processes.enable((state.rotation_process)->id);
write_powers();
}
void CommandSet::grab()
{
const int direction = atoi(sCmd.next());
if (state.grabber_state != Open && state.grabber_state != Closed) {
Serial.println(F("N - grab"));
return;
}
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.print(F("grabbing "));
Serial.println(direction);
#endif
const pid_t pid = state.grab_handler->id;
const int motor_power = 200;
/* If we're open, doing that again will bugger the vision plate */
if (direction) {
state.grabber_state = Closing;
motorForward(grabber_port, motor_power);
processes.change(pid, 300L);
} else if (state.grabber_state != Open) {
state.grabber_state = Opening;
motorBackward(grabber_port, motor_power);
processes.change(pid, 280L);
} else {
return;
}
processes.enable(pid);
processes.forward(pid);
}
void CommandSet::pixels()
{
byte red = (byte) atoi(sCmd.next());
byte green = (byte) atoi(sCmd.next());
byte blue = (byte) atoi(sCmd.next());
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.print(F("Set pixel colour to "));
Serial.print(red,HEX);
Serial.print(green,HEX);
Serial.println(blue,HEX);
#endif
for (uint16_t i = 0; i < 10; i++) {
state.strip.setPixelColor(i, red, green, blue);
}
state.strip.show();
}
int readArgument(int defaultValue = 255){
char *arg;
arg = sCmd.next();
if (arg != NULL) {
return atoi(arg);
}
else{
return defaultValue;
}
}
void CommandSet::move()
{
int new_powers[motor_count];
for (int i=0; i < motor_count; i++){
new_powers[i] = atoi(sCmd.next());
/* Apply direction correction here */
new_powers[i] *= state.motors[i]->direction;
}
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.println(F("Moving"));
#endif
/* update speeds of all drive motors */
for(int i=0; i < motor_count; i++){
state.motors[i]->power = state.motors[i]->desired_power = new_powers[i];
state.motors[i]->disp_delta = 0;
}
write_powers();
}
void CommandSet::proc_toggle()
{
pid_t pid = (pid_t) atoi(sCmd.next());
if (pid == NULL) {
Serial.println("N - ptog");
return;
}
process* proc = processes.get_by_id(pid);
if (proc == NULL) {
Serial.print(F("Unknown pid "));
Serial.println(pid);
return;
}
proc->enabled ? processes.disable(pid) : processes.enable(pid);
Serial.print(F("toggled pid "));
Serial.println(pid);
}
/*
* This method basically just allows us to execute the go command internally.
*/
void CommandSet::_go(float x_vel, float y_vel, float r_vel)
{
int power = atoi(sCmd.next());
power = power > 0 && power <= 255 ? power : 255;
float new_powers[motor_count];
new_powers[0] =
state.velo_coupling_mat[0][0] * x_vel +
state.velo_coupling_mat[0][1] * y_vel +
state.velo_coupling_mat[0][2] * r_vel;
new_powers[1] =
state.velo_coupling_mat[1][0] * x_vel +
state.velo_coupling_mat[1][1] * y_vel +
state.velo_coupling_mat[1][2] * r_vel;
new_powers[2] =
state.velo_coupling_mat[2][0] * x_vel +
state.velo_coupling_mat[2][1] * y_vel +
state.velo_coupling_mat[2][2] * r_vel;
float largest = 0.0;
for (int i=0; i<3; i++){
if (largest < fabs(new_powers[i]))
largest = fabs(new_powers[i]);
}
for (int i=0; i<3; i++){
new_powers[i] = round(new_powers[i] * (1.0/largest) * (float) power);
}
for(int i=0; i < motor_count; i++){
state.motors[i]->power = state.motors[i]->desired_power = (int) new_powers[i];
state.motors[i]->disp_delta = 0;
}
write_powers();
}
