int main(){
initializeRobot();
setDO(1, 0);
usleep(500000);
setDO(1, 1);
return 0;
}
@@method<void turnOnLED(int ledIndex)>
void turnOnLED(int ledIndex)
{
setDO(ledPins[ledIndex], 1);
Serial.print("Set digital LED "); Serial.print(ledIndex);
Serial.print(" to "); Serial.println(1);
}
void AMCSubNode::update() {
double current, velocity, position;
processStatus();
if (getState() != AMCSubNode::OperationEnabled) {
setPosition(tpdo_ptr_->actual_position);
}
if (getState() == AMCSubNode::Fault) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "Fault" << RTT::endlog();
target_state_ = ResetFault;
} else if (getState() == AMCSubNode::QuickStopActive) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "QuickStop" << RTT::endlog();
target_state_ = DisableVoltage;
} else if (getState() == AMCSubNode::NotReadyToSwitchOn) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "NotReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::SwitchOnDisabled) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "ReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::ReadyToSwitchOn) {
// go to OperationDisabled state
//RTT::log(RTT::Warning) << "OperationDisabled" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
target_state_ = SwitchOn;
} else if (getState() == AMCSubNode::SwitchedOn) {
//RTT::log(RTT::Warning) << "SwitchedOn" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
if (position_valid_) {
RTT::log(RTT::Warning) << "enable" << RTT::endlog();
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
RTT::log(RTT::Warning) << "homing" << RTT::endlog();
setControlMode(Homing);
target_state_ = BeginHoming;
homing_ = true;
}
} else if (getState() == AMCSubNode::OperationEnabled) {
//RTT::log(RTT::Warning) << "OperationEnabled" << RTT::endlog();
if (position_valid_) {
if (port_motor_position_command_.read(position) == RTT::NewData) {
setPosition(position);
}
if (port_motor_velocity_command_.read(velocity) == RTT::NewData) {
setVelocity(velocity);
}
if (port_motor_current_command_.read(current) == RTT::NewData) {
setCurrent(current);
}
// dio
bool dio;
if (port_do1_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(0);
} else {
resetDO(0);
}
}
if (port_do2_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(1);
} else {
resetDO(1);
}
}
if (port_do3_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(2);
} else {
resetDO(2);
}
}
if (port_do4_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(3);
} else {
resetDO(3);
}
}
} else {
if ((actualMode_ == Homing)) {
if (isHomingCompleted()) {
position_valid_ = true;
setPosition(tpdo_ptr_->actual_position);
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
target_state_ = BeginHoming;
}
} else {
setControlMode(Homing);
target_state_ = BeginHoming;
}
}
}
enterStateNoCheck(target_state_);
if (position_valid_) {
port_motor_position_.write(getActualPosition());
port_motor_velocity_.write(getActualVelocity());
port_motor_current_.write(getActualCurrent());
}
}
