void LBNSCommunicator::gather(){
// _logComm<<"gather:"<<_index<<std::endl;
_velocityValues.clear();
_velocityOffsets.clear();
_velocityFlips.clear();
_velocityKeys.clear();
convertVelocity(_velocityKeys,_velocityOffsets,_velocityFlips,_velocityValues);
gatherVelocity(_velocityKeys,_velocityOffsets,_velocityFlips,_velocityValues);
}
// The main motor control thread
void runMotorControl() {
if(port.open() < 0) {
::perror("Failed to start Motor Control");
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
printf("Motor Control thread running\n");
while(!stop) {
// Update motor status
// Write the status command
char who[3] = {(char)SCMD_ST,(char)1,(char)0};
port.write(who,3);
//printf("Motor status requested\n");
char tmp[2];
std::this_thread::sleep_for(std::chrono::milliseconds(300));
// Read response header
port.read(tmp,2);
char buff[(uint8_t)tmp[1]];
// Read response body
port.read(buff,(uint8_t)tmp[1]);
// Calc change in time
::gettimeofday(&this_update,NULL);
double dt = ((double)(this_update.tv_usec-last_update.tv_usec))/1000000;
// Update speeds and currents
int cps;
short cu;
for(int i = 0; i < tmp[1]/3; i++) {
::memcpy(&cps,buff+i*7+1,4);
::memcpy(&cu,buff+i*7+5,2);
double speed = convertVelocity(buff[i*7],cps);
double current = 0.01*(double)cu;
updateMotorStatus(buff[i*3],speed,current,dt);
}
// Update actions
Action a;
for(int j = 0; j < (int)actionRunning.size(); j++) {
a = actionRunning.at(j);
// Calc average velocity
double vel_avg;
for(int i = 0; i < a.num_motors; i++) {
if(a.direction[i]) {
vel_avg += stats.find(a.motor[i])->second.speed_inst;
} else {
vel_avg -= stats.find(a.motor[i])->second.speed_inst;
}
}
vel_avg /= a.num_motors;
a.speed_rt = vel_avg;
// If not measuring distance, skip
if(!a.use_dist) {
continue;
}
// Calc distance left
a.distance_remaining -= vel_avg*dt;
// If finished, set to done and erase
if(a.distance_remaining < 0) {
// Mark as done
a.status = STAT_ACTION_DONE;
// Create stop action
Action stop;
::memcpy(&stop,&a,sizeof(Action));
stop.speed = 0;
stop.use_dist = false;
stop.ovr = true;
queueAction(stop);
// Erase
actionRunning.erase(actionRunning.begin()+j);
}
}
// Set last time
::memcpy(&last_update,&this_update,sizeof(::timeval));
// Run the queue
// Wait for queue to be unlocked
while(queue_lock) {}
queue_lock = true;
// Run queue of actions
while(!actionQueue.empty()) {
printf("Dequeuing action\n");
Action tmp = actionQueue.front();
// If it does not override and conflicts with running actions, skip
if(!tmp.ovr && conflictRunning(tmp)) {
tmp.status = STAT_ACTION_NORUN;
actionQueue.pop();
continue;
}
// Stage for running and execute
tmp.status = STAT_ACTION_TORUN;
actionQueue.pop();
printf("2\n");
removeConflictingRunning(tmp);
printf("1\n");
execute(tmp);
if(tmp.status != STAT_ACTION_FAIL) {
actionRunning.push_back(tmp);
}
}
queue_lock = false;
}
running = false;
printf("Motor Control has stopped\n");
}
