static void doAction(Debugger6502* debugger, PDAction action, PDWriter* writer)
{
int t = (int)action;
switch (t)
{
case PDAction_break :
{
// On this target we can anways break so just set that we have stopped on breakpoint
printf("Fake6502Debugger: break\n");
debugger->runState = PDDebugState_stopException;
sendState(writer);
break;
}
case PDAction_run :
{
// on this target we can always start running directly again
printf("Fake6502Debugger: run\n");
debugger->runState = PDDebugState_running;
break;
}
case PDAction_step :
{
// on this target we can always stepp
printf("Fake6502Debugger: step\n");
debugger->runState = PDDebugState_trace;
sendState(writer);
break;
}
}
}
void CopyListener::onePluginWillBeRemoved(const PluginsAvailable &plugin)
{
if(plugin.category!=PluginType_Listener)
return;
int indexPlugin=0;
while(indexPlugin<pluginList.size())
{
if((plugin.path+PluginsManager::getResolvedPluginName("listener"))==pluginList.at(indexPlugin).path)
{
int index=0;
while(index<copyRunningList.size())
{
if(copyRunningList.at(index).listenInterface==pluginList.at(indexPlugin).listenInterface)
copyRunningList[index].listenInterface=NULL;
index++;
}
if(pluginList.at(indexPlugin).listenInterface!=NULL)
{
pluginList.at(indexPlugin).listenInterface->close();
delete pluginList.at(indexPlugin).listenInterface;
}
if(pluginList.at(indexPlugin).pluginLoader!=NULL)
{
pluginList.at(indexPlugin).pluginLoader->unload();
delete pluginList.at(indexPlugin).options;
}
pluginList.removeAt(indexPlugin);
sendState();
return;
}
indexPlugin++;
}
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Warning,"not found");
}
void CopyListener::newState(const Ultracopier::ListeningState &state)
{
if(stopIt)
return;
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,"start");
PluginInterface_Listener *temp=qobject_cast<PluginInterface_Listener *>(QObject::sender());
if(temp==NULL)
{
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Critical,QString("listener not located!"));
return;
}
int index=0;
while(index<pluginList.size())
{
if(temp==pluginList.at(index).listenInterface)
{
pluginList[index].state=state;
pluginList[index].inWaitOfReply=false;
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,QString("new state for the plugin %1: %2").arg(index).arg(state));
sendState(true);
return;
}
index++;
}
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Critical,QString("listener not found!"));
}
void Canvas::redraw(){
repaint();
if (model->size() > 1){
sendState(QString(model->str().c_str()));
updateStatisticInfo();
}
}
void HDMIListerner::threadFunc() {
int32_t sw_fd;
bool hdmiPlugged;
int8_t val;
setpriority(PRIO_PROCESS, 0, HAL_PRIORITY_URGENT_DISPLAY);
sw_fd = open("/sys/class/switch/hdmi/state", O_RDONLY);
if(sw_fd < 0) {
ALOGE("failed to open hdmi state file.");
return;
}
while(!mDone) {
hdmiPlugged = false;
if (read(sw_fd, &val, 1) == 1) {
if ('1' == val) {
hdmiPlugged = true;
}
}
if(hdmiPlugged != mHDMIPlugged) {
ALOGI("hdmi state changed from %d to %d",
mHDMIPlugged, hdmiPlugged);
//notify listener.
mHDMIPlugged = !mHDMIPlugged;
sendState();
}
lseek(sw_fd, 0, SEEK_SET);
usleep(1000 * 1000);
}
}
void* handle_display(void* passedClient) {
struct ClientIdPair clientIdPaire = *(struct ClientIdPair*) passedClient;
//sendId(clientIdPaire.client, clientIdPaire.id);
char message[MESSAGE_SIZE];
unsigned char action;
while (true) {
IOresult result = read(clientIdPaire.client,message,MESSAGE_SIZE);
if(result == READ_FAILED)
break;
if(result == READ_FAILED || result == CONNECTION_FINISHED)
break;
printf("message received : %s\n",message);
action = message[0];
printf("translated to %d\n",action);
if (action == ACTION_LIST) {
printf("send list\n");
sendList(clientIdPaire.client);
} else {
printf("send state");
sendState(clientIdPaire.client, action);
}
}
close(clientIdPaire.client);
return 0;
}
//
// bool sendStateToNbrs()
// Last modified: 27Aug2006
//
// Attempts to broadcast the state of the cell
// to the neighborhood of the cell, returning
// true if successful, false otherwise.
//
// Returns: true if successful, false otherwise
// Parameters: <none>
//
bool Cell::sendStateToNbrs()
{
Neighbor curr;
for (GLint i = 0; i < getNNbrs(); ++i)
if (!sendState(getNbr(i)->ID)) return false;
return true;
} // sendStateToNbrs()
int AmmBusEthernetProtocol::receiveEthernetRequests(ETHER_28J60 * e,
DS3231 *clock,
RTCDateTime * dt)
{
char* params;
if (params = e->serviceRequest())
{
//Serial.print("!");
if (strcmp(params,"state.html") == 0)
{
sendState(e,dt);
} else
{
sendPage(e);
}
byte port;
byte i;
if (strcmp(params,"?all=off") == 0)
{
for (i=0; i<8; i++)
{
port=2+i;
digitalWrite(port, HIGH);
return 1;
}
} else
if (strcmp(params,"?all=on") == 0)
{
for (i=0; i<8; i++)
{
port=2+i;
digitalWrite(port, LOW);
return 1;
}
} else
{
for (i=0; i<8; i++)
{
onStr[1] = 'a'+i;
offStr[1] = 'a'+i;
port=2+i;
if (strcmp(params, offStr) == 0)
{
digitalWrite(port, HIGH);
return 1;
} else
if (strcmp(params, onStr) == 0)
{
digitalWrite(port, LOW);
return 1;
}
}
}
}
return 0;
}
void loop(){
// Send state
int l = leftMotor->getNewTicks();
int r = rightMotor->getNewTicks();
sendState(l, r);
updateMotors();
delay(DELTA_TIME);
}
void CopyListener::resendState()
{
if(PluginsManager::pluginsManager->allPluginHaveBeenLoaded())
{
sendState(true);
pluginLoader->resendState();
}
}
void MainWindow::showStateConfigDlg()
{
if(!stateDlg){
stateDlg = new StateConfigDlg(this);
connect(stateDlg,SIGNAL(sendState(QString)),this,SLOT(setNewState(QString)));
}
stateDlg->show();
stateDlg->raise();
stateDlg->activateWindow();
}
void MainWindow::resetToIdleManual(){
curr_state = idle_manual;
Q_EMIT sendState(curr_state);
qnode.log(QNode::Info,"Reset Pressed. Re-assign Target.");
ui.pb_confirmTracking->setEnabled(false);
ui.pb_setTarget->setEnabled(true);
ui.tab_manager->setCurrentIndex(0);
ui.graphicsView->rubber->hide();
}
/* init node */
void CanNode::initCanNode(int can_idx, int node_id)
{
/* get the channels */
CAN_MSG msg;
//Digit input channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 00 60 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[0] = (int)msg.a_data[4] * 4;
/*else
QMessageBox::warning(0,"digit","read error");*/
//Digit output channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 00 62 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[1] = (int)msg.a_data[4] * 4;
/*else
QMessageBox::warning(0,"digit","read error");*/
//IP2302 has 4 digit input channels and 4 digit output channels
//sendState(00,80);
//_sleep(1000);
//if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
//{
// m_Channels[0] = 4;
// m_Channels[1] = 4;
//}
//Analog input channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 01 64 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[2] = (int)msg.a_data[4];
//Analog output channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 11 64 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[3] = (int)msg.a_data[4];
//QMessageBox::about(0,"channels",QString("channels:%1,%2,%3,%4").arg(m_Channels[0]).arg(m_Channels[1]).arg(m_Channels[2]).arg(m_Channels[3]));
sendState(01,00);
setHeartBeat(0);
setEventDriver(false);
}
/*
* ======== USBCDCMOUSE_setState ========
*/
unsigned int USBCDCMOUSE_setState(USBCDCMOUSE_State *mouseState,
unsigned int timeout)
{
unsigned int retValue = 0;
if (USB_getConnectionInformation() & USB_ENUMERATED) {
retValue = sendState(mouseState, timeout);
}
return (retValue);
}
void MainWindow::confirmTracking(){
int ret = MsgWithOKCancel("Confirming Autonomous Travel to Marked Destination");
if(ret == 1024)
{
qnode.log(QNode::Info,"Tracking confirmed and sent to robot.");
ui.pb_confirmTracking->setEnabled(false);
curr_state = auto_nav;
Q_EMIT sendState(curr_state);
}
else{
qnode.log(QNode::Info,"Please choose destination or select manual mode.");
}
}
void MainWindow::setTargetInPic()
{
if(curr_state == idle_manual){
curr_state = tracking;
Q_EMIT sendState(curr_state);
ui.pb_setTarget->setEnabled(false);
ui.pb_confirmTracking->setEnabled(true);
ui.tab_manager->setCurrentIndex(1);
}
const QImage tmp = (*(qnode.getCurrImg()));
QPixmap p = QPixmap::fromImage(tmp);
Q_EMIT mouseOverInfo(ui.graphicsView->x,ui.graphicsView->y,ui.graphicsView->xw,ui.graphicsView->yw);
qnode.log(QNode::Info,"Target Selection Complete!");
}
/**
@author Matt Olson <*****@*****.**> 404-680-0566
@brief Thread for polling and responding to pings on the RS485 network.
@param thread_id needed to start thread
*/
void *RS485_Control(void *thread_id)
{
int failCounter=0;
//init
#ifdef DEBUG
printf("Attempting initPicCom()...\n");
#endif
while ( initPicCom() != SUCCESS && failCounter < RS485_INIT_TIMEOUT_COUNTER ) {
failCounter++;
#ifdef DEBUG
printf("initPicCom() failed %d time(s)\n", failCounter);
#endif
if (failCounter == RS485_INIT_TIMEOUT_COUNTER) {
#ifdef DEBUG
printf("initPicCom() failed too many times, giving up - quitting thread...\n");
#endif
pthread_exit(NULL);
}
}
#ifdef DEBUG
printf("initPicCom() successful\n");
#endif
//Only reach here if initPicCom() is successful
Message message;
while(1) {
message = checkRS485();
if(strcmp(message.address, NULL_ADDRESS) == 0) {//strcmp returns 0 if strings are equal
continue;
}
else if(strcmp(message.address, READ_ERROR_ADDRESS)==0){
continue;//TODO: handle this somehow?
}
else{//if received something and not read_error
sendState();//for now this is needed
}
}
endPicCom();
pthread_exit(NULL);
}
void StaticElement::callback() {
if( d->staticHeader==0 || !d->watchFlags) return;
switch( core()->mode() ) {
case MulticrewCore::IdleMode: break;
case MulticrewCore::HostMode:
if( (d->staticHeader->image_flags & STATIC_FLAGS_MASK)!=d->oldFlags ) {
/*dout << "Static callback " << id()
<< " = " << to_string( d->staticHeader->image_flags, 2 )
<< std::endl;*/
d->oldFlags = d->staticHeader->image_flags & STATIC_FLAGS_MASK;
sendState();
}
break;
case MulticrewCore::ClientMode:
d->staticHeader->image_flags = (d->staticHeader->image_flags & ~STATIC_FLAGS_MASK) | d->oldFlags;
break;
}
}
void MainWindow::connectToROS() {
if ( !qnode.init() ) {
showNoMasterMessage();
} else {
ui.pb_connect->setEnabled(false);
curr_state = idle_manual;
Q_EMIT sendState(curr_state);
timer = new QTimer(this);
lblTimer = new QTimer(this);
connect(timer, SIGNAL(timeout()), &qnode, SLOT(update()));
connect(lblTimer, SIGNAL(timeout()), this, SLOT(updateLabel()));
timer->start(10);
lblTimer->start(10);
}
}
// wird nach dem Starten dauerhaft ausgeführt
void loop() {
// Einschalter auslesen
if (debounceBtnOn.update() && debounceBtnOn.read()) {
changeStateTo(STATE_ON);
}
// Ausschalter auslesen
if (debounceBtnOff.update() && debounceBtnOff.read()) {
changeStateTo(STATE_OFF);
}
// Auswertung des aktuellen Zustandes
// ggf Zustand wechseln
if (state_current == STATE_ON) {
if (calcStateTime() >= TIME_HALF) {
changeStateTo(STATE_HALF);
}
} else if (state_current == STATE_HALF && calcStateTime() >= TIME_OFF) {
changeStateTo(STATE_OFF);
}
// kommunizieren
sendState();
delay(10);
}
void cmMyBlind::poll(void) {
updateState(); // check if something is to be set on the PWM channel
sendState(); // check if there is some status to send
stateMachine_poll(0, 0, 0);
}
void CopyListener::allPluginIsloaded()
{
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,"with value: "+QString::number(pluginList.size()>0));
sendState(true);
reloadClientList();
}
void Joystick_::releaseButton(uint8_t button)
{
bitClear(buttons, button);
if (autoSendState) sendState();
}
void Joystick_::setThrottle(uint16_t value)
{
throttle = value;
if (autoSendState) sendState();
}
void Joystick_::setRudder(uint16_t value)
{
rudder = value;
if (autoSendState) sendState();
}
void Joystick_::setZAxisRotation(int16_t value)
{
zAxisRotation = value;
if (autoSendState) sendState();
}
void Walker::commenceWalking(balance_state_t &parent_state, nudge_state_t &state, balance_gains_t &gains)
{
int timeIndex=0, nextTimeIndex=0, prevTimeIndex=0;
keepWalking = true;
size_t fs;
zmp_traj_t *prevTrajectory, *currentTrajectory, *nextTrajectory;
prevTrajectory = new zmp_traj_t;
currentTrajectory = new zmp_traj_t;
nextTrajectory = new zmp_traj_t;
memset( prevTrajectory, 0, sizeof(*prevTrajectory) );
memset( currentTrajectory, 0, sizeof(*currentTrajectory) );
memset( nextTrajectory, 0, sizeof(*nextTrajectory) );
// TODO: Consider making these values persistent
memset( &state, 0, sizeof(state) );
memcpy( &bal_state, &parent_state, sizeof(bal_state) );
bal_state.m_balance_mode = BAL_ZMP_WALKING;
bal_state.m_walk_mode = WALK_WAITING;
bal_state.m_walk_error = NO_WALK_ERROR;
sendState();
currentTrajectory->reuse = true;
fprintf(stdout, "Waiting for first trajectory\n"); fflush(stdout);
ach_status_t r;
do {
struct timespec t;
clock_gettime( ACH_DEFAULT_CLOCK, &t );
t.tv_sec += 1;
r = ach_get( &zmp_chan, currentTrajectory, sizeof(*currentTrajectory), &fs,
&t, ACH_O_WAIT | ACH_O_LAST );
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
} while(!daemon_sig_quit && keepWalking && (r==ACH_TIMEOUT
|| !currentTrajectory->reuse) ); // TODO: Replace this with something more intelligent
if(!keepWalking || !currentTrajectory->reuse) // TODO: Take out the reuse condition here
{
bal_state.m_walk_mode = WALK_INACTIVE;
sendState();
return;
}
if(!daemon_sig_quit)
fprintf(stdout, "First trajectory acquired\n");
daemon_assert( !daemon_sig_quit, __LINE__ );
bal_state.m_walk_mode = WALK_INITIALIZING;
sendState();
// Get the balancing gains from the ach channel
ach_get( ¶m_chan, &gains, sizeof(gains), &fs, NULL, ACH_O_LAST );
hubo.update(true);
// Set all the joints to the initial posiiton in the trajectory
// using the control daemon to interpolate in joint space.
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
// Don't worry about where these joint are
if( LF1!=i && LF2!=i && LF3!=i && LF4!=i && LF5!=i
&& RF1!=i && RF2!=i && RF3!=i && RF4!=i && RF5!=i
&& NK1!=i && NK2!=i && NKY!=i && LWR!=i && RWR!=i && RWY!=i && RWP!=i) //FIXME
{
hubo.setJointAngle( i, currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i] );
hubo.setJointNominalSpeed( i, 0.4 );
hubo.setJointNominalAcceleration( i, 0.4 );
}
//std::cout << jointNames[i] << " = " << currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i] << "\n";
}
hubo.setJointNominalSpeed( RKN, 0.8 );
hubo.setJointNominalAcceleration( RKN, 0.8 );
hubo.setJointNominalSpeed( LKN, 0.8 );
hubo.setJointNominalAcceleration( LKN, 0.8 );
// hubo.setJointAngle( RSR, currentTrajectory->traj[0].angles[RSR] + hubo.getJointAngleMax(RSR) );
// hubo.setJointAngle( LSR, currentTrajectory->traj[0].angles[LSR] + hubo.getJointAngleMin(LSR) );
hubo.sendControls();
// Wait specified time for joints to get into initial configuration,
// otherwise time out and alert user.
double m_maxInitTime = 10;
double biggestErr = 0;
int worstJoint=-1;
double dt, time, stime; stime=hubo.getTime(); time=hubo.getTime();
double norm = m_jointSpaceTolerance+1; // make sure this fails initially
while( !daemon_sig_quit && (norm > m_jointSpaceTolerance && time-stime < m_maxInitTime)) {
// while(false) { // FIXME TODO: SWITCH THIS BACK!!!
hubo.update(true);
norm = 0;
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
double err=0;
// Don't worry about waiting for these joints to get into position.
if( LF1!=i && LF2!=i && LF3!=i && LF4!=i && LF5!=i
&& RF1!=i && RF2!=i && RF3!=i && RF4!=i && RF5!=i
&& NK1!=i && NK2!=i && NKY!=i && LWR!=i && RWR!=i && RWY!=i && RWP!=i) //FIXME
err = (hubo.getJointAngleState( i )-currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i]);
// if( LSR == i )
// err -= hubo.getJointAngleMin(i);
// if( RSR == i )
// err -= hubo.getJointAngleMax(i);
norm += err*err;
if( fabs(err) > fabs(biggestErr) )
{
biggestErr = err;
worstJoint = i;
}
}
time = hubo.getTime();
}
// Print timeout error if joints don't get to initial positions in time
if( time-stime >= m_maxInitTime )
{
fprintf(stderr, "Warning: could not reach the starting Trajectory within %f seconds\n"
" -- Biggest error was %f radians in joint %s\n",
m_maxInitTime, biggestErr, jointNames[worstJoint] );
keepWalking = false;
bal_state.m_walk_error = WALK_INIT_FAILED;
}
timeIndex = 1;
bool haveNewTrajectory = false;
if( keepWalking )
fprintf(stdout, "Beginning main walking loop\n"); fflush(stdout);
while(keepWalking && !daemon_sig_quit)
{
haveNewTrajectory = checkForNewTrajectory(*nextTrajectory, haveNewTrajectory);
ach_get( ¶m_chan, &gains, sizeof(gains), &fs, NULL, ACH_O_LAST );
hubo.update(true);
bal_state.m_walk_mode = WALK_IN_PROGRESS;
dt = hubo.getTime() - time;
time = hubo.getTime();
if( dt <= 0 )
{
fprintf(stderr, "Something unnatural has happened... %f\n", dt);
continue;
}
if( timeIndex==0 )
{
bal_state.m_walk_error = NO_WALK_ERROR;
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, prevTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex == currentTrajectory->periodEndTick && haveNewTrajectory )
{
if( validateNextTrajectory( currentTrajectory->traj[timeIndex],
nextTrajectory->traj[0], dt ) )
{
nextTimeIndex = 0;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
nextTrajectory->traj[nextTimeIndex],
state, gains, dt );
memcpy( prevTrajectory, currentTrajectory, sizeof(*prevTrajectory) );
memcpy( currentTrajectory, nextTrajectory, sizeof(*nextTrajectory) );
fprintf(stderr, "Notice: Swapping in new trajectory\n");
}
else
{
fprintf(stderr, "WARNING: Discontinuous trajectory passed in. Walking to a stop.\n");
bal_state.m_walk_error = WALK_FAILED_SWAP;
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
haveNewTrajectory = false;
}
else if( timeIndex == currentTrajectory->periodEndTick && currentTrajectory->reuse )
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
currentTrajectory->reuse = false;
if( currentTrajectory->reuse == true )
nextTimeIndex = currentTrajectory->periodStartTick;
else
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex < currentTrajectory->count-1 )
{
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex == currentTrajectory->count-1 && haveNewTrajectory )
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
if( keepWalking )
{
if( validateNextTrajectory( currentTrajectory->traj[timeIndex],
nextTrajectory->traj[0], dt ) )
{
bal_state.m_walk_error = NO_WALK_ERROR;
nextTimeIndex = 0;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
nextTrajectory->traj[nextTimeIndex],
state, gains, dt );
memcpy( prevTrajectory, currentTrajectory, sizeof(*prevTrajectory) );
memcpy( currentTrajectory, nextTrajectory, sizeof(*nextTrajectory) );
}
else
{
bal_state.m_walk_mode = WALK_WAITING;
bal_state.m_walk_error = WALK_FAILED_SWAP;
fprintf(stderr, "WARNING: Discontinuous trajectory passed in. Discarding it.\n");
}
haveNewTrajectory = false;
}
}
else
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
}
prevTimeIndex = timeIndex;
timeIndex = nextTimeIndex;
sendState();
}
bal_state.m_walk_mode = WALK_INACTIVE;
sendState();
}
void Joystick_::setHatSwitch(int8_t hatSwitchIndex, int16_t value)
{
hatSwitch[hatSwitchIndex % 2] = value;
if (autoSendState) sendState();
}
void Joystick_::setYAxis(int16_t value)
{
yAxis = value;
if (autoSendState) sendState();
}
void StaticElement::sendFullState() {
sendState();
}