// Implements steps according to the current speed
// You must call this at least once per step
// returns true if a step occurred
boolean EightAccelStepper::runSpeed()
{
unsigned long time = millis();
if (time > getLastStepTime() + getStepInterval())
{
if (getSpeed() > 0)
{
// Clockwise
setCurrentPos(getCurrentPos() + 1);
}
else if (getSpeed() < 0)
{
// Anticlockwise
setCurrentPos(getCurrentPos() - 1);
}
if (getPins() == 8) {
step(getCurrentPos() & 0x7); // Bottom 3 bits (same as mod 8, but works with + and - numbers)
} else {
step(getCurrentPos() & 0x3); // Bottom 2 bits (same as mod 4, but works with + and - numbers)
}
setLastStepTime(time);
return true;
}
else
return false;
}
void RobotManager::moveRobot(WayPoint* wayPoint) {
double x,y;
double deltaX, deltaY, startX, startY;
x = wayPoint->x_Coordinate;
y = wayPoint->y_Coordinate - Y_FACTOR;
xyPosition targetPos(x, y);
cout<<"Total Completed "<<wpm.getCompletedPercent()<<"%"<<endl;
getCurrentPos();
//do not move x,y if smaller than the delta
deltaX = abs(targetPos.first - currPos.first);
deltaY = abs(targetPos.second - currPos.second);
if(deltaX < deltaTolerance)
deltaX = 0;
if(deltaY < deltaTolerance)
deltaY = 0;
getCurrentPos();
startX = currPos.first;
startY = currPos.second;
//move to next point
robot->SetSpeed(linearSpeed, 0);
while (abs(startX - currPos.first) < deltaX-0.2 ||
abs(startY - currPos.second) < deltaY-0.2) {
getCurrentPos();
}
// slow down before reaching the target
robot->SetSpeed(0.05 * linearSpeed, 0);
moveToNextYaw(targetPos, wayPoint->yaw);
}
void Teleport::beginContact(GameObject* go_ptr, b2Fixture* check) {
if (go_ptr==NULL) {
return;
}
if (check!=my_check) return;
if (find(incoming.begin(), incoming.end(), go_ptr) == incoming.end()) {
Teleport* next=dynamic_cast<Teleport*>(calculate_next());
if (next!=NULL) {
next->notify(go_ptr);
b2Vec2 target=next->getCurrentPos();
target+=go_ptr->getCurrentPos()-getCurrentPos();
int flipped_delta=next->flipped-flipped;
if (flipped_delta<0) flipped_delta=4+flipped_delta;
b2Vec2 targetvelocity=go_ptr->getLinearVelocity();
for (;flipped_delta>0;flipped_delta--) {
targetvelocity=b2Vec2(targetvelocity.y,-targetvelocity.x);
}
flipped_delta=next->flipped-flipped;
if (flipped_delta%2==0) {
go_ptr->teleport(target, targetvelocity, false);
}
else {
go_ptr->teleport(target, targetvelocity, true);
}
}
}
}
void DnDWidget::mouseReleaseEvent(QMouseEvent *event)
{
Q_UNUSED(event);
if (hadDrag)
{
hadDrag = false;
moveVerticalToNewPos(getCurrentPos());
}
}
void RobotManager::moveForward(xyPosition targetPos, double yaw, int direction, bool yawOnStart) {
getCurrentPos();
// change YAW for to next point
if(yawOnStart)
moveToNextYaw(targetPos, yaw);
robot->SetSpeed(linearSpeed, 0);
while (abs(currPos.first - targetPos.first) > 10 * linearTolerance ||
abs(currPos.second - targetPos.second) > 10 * linearTolerance) {
getCurrentPos();
}
// slow down before reaching the target
robot->SetSpeed(0.05 * linearSpeed, 0);
while (abs(currPos.first - targetPos.first) > linearTolerance ||
abs(currPos.second - targetPos.second) > linearTolerance)
{ getCurrentPos();
}
moveToNextYaw(targetPos, yaw);
}
RGBApixel Bitmap::getPixel(int row, int col) const
{
RGBApixel rgbaPixel;
int c = getCurrentPos(row, col);
if(c != -1)
{
rgbaPixel.red = p_pixelArray[c + 0];
rgbaPixel.green = p_pixelArray[c + 1];
rgbaPixel.blue = p_pixelArray[c + 2];
}
return(rgbaPixel);
}
void Bitmap::setPixelLow(int row, int col, int red, int green, int blue, int alpha)
{
int c = getCurrentPos(row, col);
if(c != -1)
{
DWORD pixelArraySize = calculatePixelArraySize();
if(p_pixelArray == NULL)
{
p_pixelArray = (unsigned char*)malloc(pixelArraySize);
memset(p_pixelArray, 0xff, pixelArraySize);
}
p_pixelArray[ c + 0 ] = (unsigned char)red;
p_pixelArray[ c + 1 ] = (unsigned char)green;
p_pixelArray[ c + 2 ] = (unsigned char)blue;
}
}
void Player::seek(unsigned int percent)
{
if (percent > 100)
{
qDebug() << "Invalide seek value, must be [O;100]";
return;
}
int currentSongId = getCurrentPos();
if (currentSongId < 0 || currentSongId >= playlist.tracks.size())
{
return;
}
unsigned int totalTime = playlist.tracks.at(currentSongId).duration;
EMSPlayerCmd cmd;
cmd.action = ACTION_SEEK;
cmd.uintValue = (totalTime*percent) / 100;
mutex.lock();
queue.enqueue(cmd);
mutex.unlock();
cmdAvailable.release(1);
}
void ArmPlat::TimerHit()
{
uint32_t data;
if (!isConnected())
{
SetTimer(POLLMS);
return;
}
if ( port == -1 )
{
if ( !portWarned )
{
DEBUG( INDI::Logger::DBG_WARNING, "Port must be selected (and configuration saved)" );
portWarned = true;
}
SetTimer(POLLMS);
return;
}
else
portWarned = false;
bool rc = getCurrentPos( &data );
if (rc)
{
if ( data != FocusAbsPosN[0].value )
{
FocusAbsPosN[0].value = data;
IDSetNumber(&FocusAbsPosNP, nullptr);
}
else
isMoving = false;
if (FocusAbsPosNP.s == IPS_BUSY || FocusRelPosNP.s == IPS_BUSY)
{
if (isMoving == false)
{
FocusAbsPosNP.s = IPS_OK;
FocusRelPosNP.s = IPS_OK;
IDSetNumber(&FocusRelPosNP, nullptr);
IDSetNumber(&FocusAbsPosNP, nullptr);
LOG_INFO("Focuser reached requested position.");
}
}
}
if (isMoving == false)
{
bool rc = getCurrentTemp( &data );
if (rc)
{
if ( data != TemperatureN[0].value )
{
TemperatureN[0].value = data;
IDSetNumber(&TemperatureNP, nullptr);
}
}
}
SetTimer(POLLMS);
}
/* Execute the given command. In fact, it just will ask MPD to do it.
* You will have to check the next "status" or "playlist" update (using the corresponding slot)
* to know if the command has really been executed.
*/
void Player::executeCmd(EMSPlayerCmd cmd)
{
bool waitResponse = true; /* Wait the responase by default */
bool error = false;
if (conn == NULL)
{
return;
}
/* Dispatch execution depending on the action */
switch (cmd.action)
{
case ACTION_ADD:
{
if(searchTrackInPlaylist(cmd.track) >= 0)
{
qDebug() << "Do not add track in the playlist as it already exist";
return;
}
QString filename = getMPDFilename(cmd.track);
mpd_send_add(conn, filename.toStdString().c_str());
break;
}
case ACTION_DEL:
{
int pos = searchTrackInPlaylist(cmd.track);
if (pos >= 0)
{
mpd_send_delete(conn, pos);
}
break;
}
case ACTION_DEL_ALL:
{
mpd_send_clear(conn);
break;
}
case ACTION_PLAY_POS:
{
mutex.lock();
int size = playlist.tracks.size();
mutex.unlock();
if (size <= 0 || cmd.uintValue >= (unsigned int)size)
{
qDebug() << "Asked to play a track after the end of the current playlist";
error = true;
break;
}
mpd_send_play_pos(conn, cmd.uintValue);
break;
}
case ACTION_PLAY_TRACK:
{
int position = searchTrackInPlaylist(cmd.track);
if (position < 0)
{
qDebug() << "Asked to play a track which is not in the current playlist";
error = true;
break;
}
mpd_send_play_pos(conn, position);
break;
}
case ACTION_PLAY:
{
mpd_send_play(conn);
break;
}
case ACTION_SEEK:
{
/* Get current position */
int songId = getCurrentPos();
if (songId >= 0)
{
mpd_send_seek_pos(conn, songId, cmd.uintValue);
}
break;
}
case ACTION_PAUSE:
{
mpd_send_pause(conn, true);
break;
}
case ACTION_TOGGLE:
{
mpd_send_toggle_pause(conn);
break;
}
case ACTION_STOP:
{
mpd_send_stop(conn);
break;
}
case ACTION_NEXT:
{
mpd_send_next(conn);
break;
}
case ACTION_PREV:
{
mpd_send_previous(conn);
break;
}
case ACTION_REPEAT:
{
mutex.lock();
bool repeatTmp = status.repeat;
mutex.unlock();
if (cmd.boolValue != repeatTmp)
{
mpd_send_repeat(conn, cmd.boolValue);
}
break;
}
case ACTION_RANDOM:
{
mutex.lock();
bool randomTmp = status.random;
mutex.unlock();
if (cmd.boolValue != randomTmp)
{
mpd_send_random(conn, cmd.boolValue);
}
break;
}
case ACTION_ENABLE_OUTPUT:
{
if (cmd.uintValue >= 1)
{
mpd_send_enable_output(conn, cmd.uintValue-1);
}
break;
}
case ACTION_DISABLE_OUTPUT:
{
if (cmd.uintValue >= 1)
{
mpd_send_disable_output(conn, cmd.uintValue-1);
}
break;
}
default:
{
qCritical() << "Unhandled action in the current command.";
waitResponse = false;
break;
}
}
if (waitResponse && !mpd_response_finish(conn))
{
error = true;
qCritical() << "MPD could not execute the current command.";
enum mpd_error errorMpd = mpd_connection_get_error(conn);
if (errorMpd == MPD_ERROR_SERVER ||
errorMpd == MPD_ERROR_ARGUMENT) /* Problem with the command */
{
QString errorMessage = QString::fromUtf8(mpd_connection_get_error_message(conn));
qCritical() << "Command error : " << errorMessage;
if (!mpd_connection_clear_error(conn))
{
qCritical() << "This error cannot be cleared, reconnecting...";
connectToMpd();
}
}
else if (errorMpd == MPD_ERROR_TIMEOUT ||
errorMpd == MPD_ERROR_RESOLVER ||
errorMpd == MPD_ERROR_MALFORMED ||
errorMpd == MPD_ERROR_CLOSED ) /* Assume there is a connection problem, try to reconnect... */
{
QString errorMessage = QString::fromUtf8(mpd_connection_get_error_message(conn));
qCritical() << "Connexion error : " << errorMessage;
qCritical() << "Reconnecting...";
connectToMpd();
mutex.lock();
queue.push_front(cmd);
mutex.unlock();
cmdAvailable.release(1);
}
}
/* Post-action depending on the command
* Do the minimum here as the whole status will be
* retrieve here. But for playlist ADD/DEL, we need to
* store the EMSTrack structure.
*/
if(!error)
{
switch (cmd.action)
{
case ACTION_ADD:
{
EMSPlaylist newPlaylist;
mutex.lock();
playlist.tracks.append(cmd.track);
newPlaylist = playlist;
mutex.unlock();
emit playlistChanged(newPlaylist);
break;
}
case ACTION_DEL:
{
int pos = searchTrackInPlaylist(cmd.track);
if (pos >= 0)
{
EMSPlaylist newPlaylist;
mutex.lock();
playlist.tracks.removeAt(pos);
newPlaylist = playlist;
mutex.unlock();
emit playlistChanged(newPlaylist);
}
break;
}
case ACTION_DEL_ALL:
{
EMSPlaylist newPlaylist;
mutex.lock();
playlist.tracks.clear();
newPlaylist = playlist;
mutex.unlock();
emit playlistChanged(newPlaylist);
break;
}
case ACTION_ENABLE_OUTPUT:
{
mutex.lock();
for(int i=0; i<outputs.size(); i++)
{
if (outputs.at(i).id_mpd == cmd.uintValue)
{
EMSSndCard card = outputs.at(i);
card.enabled = true;
outputs.replace(i, card);
}
}
emit outputsChanged(outputs);
mutex.unlock();
break;
}
case ACTION_DISABLE_OUTPUT:
{
mutex.lock();
for(int i=0; i<outputs.size(); i++)
{
if (outputs.at(i).id_mpd == cmd.uintValue)
{
EMSSndCard card = outputs.at(i);
card.enabled = false;
outputs.replace(i, card);
}
}
emit outputsChanged(outputs);
mutex.unlock();
break;
}
default:
break;
}
}
}
void DnDWidget::mousePressEvent(QMouseEvent *event)
{
clickOffset = event->pos();
oldPos = getCurrentPos();
}