void IRp6Regulator::updateHook() {
if (NewData == deltaInc_in.read(deltaIncData)) {
update_hook_iteration_number_++;
if (update_hook_iteration_number_ <= 1) {
deltaIncData = 0.0;
}
if (NewData == desired_position_.read(desired_position_new_)) {
new_position_iteration_number_++;
if (new_position_iteration_number_ <= 1) {
desired_position_old_ = desired_position_new_;
}
}
if (NewData == synchro_state_in_.read(synchro_state_new_)) {
if (synchro_state_new_ != synchro_state_old_) {
desired_position_old_ = desired_position_new_;
synchro_state_old_ = synchro_state_new_;
}
}
desired_position_increment_ =
(desired_position_new_ - desired_position_old_)
* (enc_res_ / (2.0 * M_PI));
if (fabs(desired_position_increment_) > max_desired_increment_) {
std::cout << "very high pos_inc_: " << reg_number_ << " pos_inc: "
<< desired_position_increment_ << std::endl;
emergency_stop_out_.write(true);
}
desired_position_old_ = desired_position_new_;
if (!debug_) {
int output = doServo(desired_position_increment_, deltaIncData);
/*
std::cout << std::dec << GREEN << "output: " << output << " pos_inc: "
<< desired_position_increment_ << " inp_inc: " << deltaIncData
<< RESET << std::endl;
if (iteration_number_ > 2000) {
output = 0;
}
*/
computedPwm_out.write(output);
} else {
computedPwm_out.write(0.0);
}
}
}
void FileInputControl::updateHook() {
if (RTT::NewData == deltaInc_in.read(deltaIncData)) {
update_hook_iteration_number_++;
if (update_hook_iteration_number_ <= 1) {
deltaIncData = 0.0;
}
if (RTT::NewData == desired_position_.read(desired_position_new_)) {
new_position_iteration_number_++;
if (new_position_iteration_number_ <= 1) {
desired_position_old_ = desired_position_new_;
}
}
if (RTT::NewData == synchro_state_in_.read(synchro_state_new_)) {
if (synchro_state_new_ != synchro_state_old_) {
desired_position_old_ = desired_position_new_;
synchro_state_old_ = synchro_state_new_;
}
}
desired_position_increment_ =
(desired_position_new_ - desired_position_old_)
* (enc_res_ / (2.0 * M_PI));
if (fabs(desired_position_increment_) > max_desired_increment_) {
std::cout << "very high pos_inc_: " << reg_number_ << " pos_inc: "
<< desired_position_increment_ << std::endl;
emergency_stop_out_.write(true);
}
desired_position_old_ = desired_position_new_;
if (!debug_) {
int output;
if (synchro_state_old_) {
switch (type) {
case current:
output = doServo_fcc(desired_position_increment_, deltaIncData);
break;
case increment:
output = doServo_fic(desired_position_increment_, deltaIncData);
break;
default:
output = doServo(desired_position_increment_, deltaIncData);
break;
}
} else {
output = doServo(desired_position_increment_, deltaIncData);
}
/*
std::cout << std::dec << GREEN << "output: " << output << " pos_inc: "
<< desired_position_increment_ << " inp_inc: " << deltaIncData
<< RESET << std::endl;
if (iteration_number_ > 2000) {
output = 0;
}
*/
computedPwm_out.write(output);
} else {
computedPwm_out.write(0.0);
}
}
}
/* Servo Timer Interrupt 4 --------------------------------------------------*/
void TIM4_IRQHandler(void)
{
/* Clear TIM4 update interrupt */
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
doServo();
}
