// Ёкспортируемые функции
int _malloc (VirtualAddress * ptr, size_t szBlock){
MemoryBlock * freeBlock;
MemoryBlock * newBlock;
VirtualAddress adress;
//printf("malloc\n");
if(!szBlock){
return -1;
}
freeBlock = findFreeBlock(szBlock);
if(!freeBlock){
return -2;
}
newBlock = (MemoryBlock *)malloc(sizeof(MemoryBlock));
adress = freeBlock->blockAdress;
freeBlock->blockAdress = adress + szBlock;
freeBlock->blockSize = freeBlock->blockSize - szBlock;
newBlock->blockAdress = adress;
newBlock->blockSize = szBlock;
newBlock->isFree = false;
if(!(freeBlock->previous)){
newBlock->next = freeBlock;
freeBlock->previous = newBlock;
newBlock->previous = NULL;
memoryManager.firstMemoryBlock = newBlock;
}
else{
switchBlocks(freeBlock, newBlock);
}
*ptr = adress;
return 0;
}
void SarkofagSupervisor::setSynchronized() {
robot_state_ = SYNCHRONIZED;
// switch Regulators
for (int i = 0; i < number_of_servos_; i++) {
disable_vec_.clear();
enable_vec_.clear();
enable_vec_.push_back(regulators_names_[i]);
RTT::OperationCaller<
bool(const std::vector<std::string> &disable_block_names,
const std::vector<std::string> &enable_block_names,
const bool strict, const bool force)> switchBlocks;
switchBlocks = Scheme->getOperation("switchBlocks");
switchBlocks.setCaller(this->engine());
switchBlocks(disable_vec_, enable_vec_, true, true);
std::cout << regulators_names_[i] << ": ENABLED" << std::endl;
}
}
void SarkofagSupervisor::updateHook() {
switch (robot_state_) {
case NOT_OPERATIONAL:
for (int i = 0; i < number_of_servos_; i++) {
if (servo_state_[i] != NOT_SYNCHRONIZED) {
RTT::Attribute<ECServoState> * servo_ec_state = (RTT::Attribute<ECServoState> *) EC
->provides(services_names_[i])->getAttribute("state");
ec_servo_state_ = servo_ec_state->get();
if (auto_) {
// set "enable" if powered on
if (ec_servo_state_ == SWITCH_ON) {
RTT::OperationCaller<bool(void)> enable;
enable = EC->provides(services_names_[i])->getOperation("enable");
enable.setCaller(this->engine());
enable();
}
}
// servo enabled
if (ec_servo_state_ == OPERATION_ENABLED) {
std::cout << services_names_[i] << ": ENABLED" << std::endl;
servo_state_[i] = NOT_SYNCHRONIZED;
++servos_state_changed_;
}
}
}
// all servos enabled
if (servos_state_changed_ == number_of_servos_) {
robot_state_ = NOT_SYNCHRONIZED;
std::cout << "ROBOT NOT SYNCHRONIZED" << std::endl;
servos_state_changed_ = 0;
}
break;
case NOT_SYNCHRONIZED:
if (auto_) robot_state_ = SYNCHRONIZING;
break;
case SYNCHRONIZING:
for (int i = 0; i < number_of_servos_; i++) {
RTT::Attribute<ECServoState> * servo_state = (RTT::Attribute<ECServoState> *) EC
->provides(services_names_[i])->getAttribute("state");
ec_servo_state_ = servo_state->get();
if (ec_servo_state_ == OPERATION_ENABLED) {
switch (servo_state_[i]) {
case NOT_SYNCHRONIZED:
if (i == last_servo_synchro_) {
RTT::OperationCaller<bool(void)> beginHoming;
beginHoming = EC->provides(services_names_[i])->getOperation("beginHoming");
beginHoming.setCaller(this->engine());
beginHoming();
servo_state_[i] = SYNCHRONIZING;
std::cout << services_names_[i] << ": SYNCHRONIZING" << std::endl;
}
break;
case SYNCHRONIZING:
RTT::Attribute<bool> * homing = (RTT::Attribute<bool> *) EC->provides(
services_names_[i])->getAttribute("homing_done");
if (homing->get()) {
servo_state_[i] = SYNCHRONIZED;
std::cout << services_names_[i] << ": SYNCHRONIZED" << std::endl;
last_servo_synchro_ = i+1;
++servos_state_changed_;
// switch Regulator
disable_vec_.clear();
enable_vec_.clear();
enable_vec_.push_back(regulators_names_[i]);
RTT::OperationCaller<
bool(const std::vector<std::string> &disable_block_names,
const std::vector<std::string> &enable_block_names,
const bool strict, const bool force)> switchBlocks;
switchBlocks = Scheme->getOperation("switchBlocks");
switchBlocks.setCaller(this->engine());
switchBlocks(disable_vec_, enable_vec_, true, true);
std::cout << regulators_names_[i] << ": ENABLED" << std::endl;
}
break;
}
}
}
// all servos synhronized
if (servos_state_changed_ == number_of_servos_) {
robot_state_ = SYNCHRONIZED;
std::cout << "ROBOT SYNCHRONIZED" << std::endl;
servos_state_changed_ = 0;
}
break;
case SYNCHRONIZED:
robot_state_ = RUNNING;
std::cout << "ROBOT READY" << std::endl;
break;
case RUNNING:
if (fault_autoreset_) {
resetFaultAll();
enableAll();
}
break;
default:
break;
}
}
