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()); } }