void run() {
if(idle()) {
return;
}
unsigned long new_time = millis();
unsigned long delta = new_time - last_time;
last_time = new_time;
// If it's a time command (8bx000 0xxx)
if((cmds[cmd_at] & 0x78) == 0x00) {
uint16_t *data = cmdArg(1, uint16_t);
if((cmds[cmd_at] & 0x7f) == BRAKE) {
for(uint8_t i = 0; i < 4; i++) {
if(io::motor_positions[i] < -3) {
io::forward(i + 1, 0x7f);
} else if(io::motor_positions[i] > 3) {
io::backward(i + 1, 0x7f);
} else {
io::brake(i + 1);
}
}
}
if(delta >= *data) {
*data = 0;
finish(true);
} else {
*data -= delta;
}
} else if((cmds[cmd_at] & 0x7f) == STRAIT || (cmds[cmd_at] & 0x7f) == ARC) {
int16_t data = *cmdArg(1, int16_t);
if(io::dist() >= abs(data)) {
finish(true);
} else {
io::motorSet(
data > 0 ? MOVE_RIGHT : MOVE_LEFT,
(cmds[cmd_at] & 0x7f) == STRAIT ?
io::adjustedMotorPowers(1, 1) :
io::adjustedMotorPowers(
*cmdArg(3, uint16_t),
*cmdArg(5, uint16_t)));
}
} else if((cmds[cmd_at] & 0x7f) == SPIN) {
int16_t data = *cmdArg(1, int16_t);
if(io::rotDist() >= abs(data)) {
finish(true);
}
}
}
void start() {
for(int i = 0; i < 4; i++) {
io::motor_positions[i] = 0;
}
if(idle()) {
// Should really be stopped already, but hey, let's make sure.
io::brakeAll();
return;
}
switch(cmds[cmd_at] & 0x7f) {
case GRABBER_OPEN:
io::backward(MOTOR_GRABBERS, 255);
break;
case GRABBER_CLOSE:
io::forward(MOTOR_GRABBERS, 50);
break;
case GRABBER_FORCE:
io::forward(MOTOR_GRABBERS, 255);
break;
case HOLD_SPIN:
case SPIN:
io::motorSet(*cmdArg(1, int16_t) > 0 ? MOVE_CW : MOVE_CC,
io::POWER_FULL);
case STRAIT:
case ARC:
if(state::grabbers_open) {
io::backward(MOTOR_GRABBERS, 30);
} else {
io::forward(MOTOR_GRABBERS, 30);
}
break;
case NOP:
cmd_at++;
start();
break;
case KICK:
digitalWrite(PIN_KICKER, HIGH);
break;
case SPD_SET:
state::speed = *cmdArg(1, uint8_t);
cmd_at += 2;
start();
break;
}
last_time = millis();
}
void PGManager::stopSequence( const StepQueue& seq )
{
if(!pgEntity) {
sotERROR <<"PG not set" << std::endl;
return;
}
std::ostringstream cmdstd; cmdstd << ":StopOnLineStepSequencing";
std::ostringstream os;
std::istringstream cmdArg( cmdstd.str() );
}
void PGManager::introduceStep( StepQueue& queue )
{
if(!pgEntity) {
sotERROR << "Walk plugin not found. " << std::endl;
return;
}
const FootPrint& lastStep = queue.getLastStep();
std::string cmdLine = "addStep";
std::ostringstream cmdArgIn, os;
cmdArgIn << lastStep.x << " " << lastStep.y << " " << lastStep.theta;
std::istringstream cmdArg( cmdArgIn.str() );
}
void PGManager::startSequence( const StepQueue& seq )
{
if(!pgEntity) {
sotERROR <<"PG not set" << std::endl;
return;
}
std::ostringstream cmdstd; cmdstd << ":StartOnLineStepSequencing ";
std::ostringstream os;
for( unsigned int i = 0; i < seq.size(); ++i ) {
const FootPrint& fp = seq.getStep(i);
cmdstd << fp.x << " " << fp.y << " " << fp.theta << " ";
}
std::istringstream cmdArg( cmdstd.str() );
std::istringstream emptyArg;
sotDEBUG(15) << "Cmd: " << cmdstd.str() << std::endl;
}
