void Actuator::update() {
if(!simulated) {
/*
for(int i=0; i<10; i++) {
togglePin(16);
delay(200);
}
delay(200);
*/
length = readLength();
//length = 0;
float error = length - targetLength;
writePWM(error * kP);
}
else {
length = targetLength;
}
}
void SoemEBox::update()
{
m_input = *((in_eboxt*) (m_datap->inputs));
EBOXOut out_msg;
std::bitset < 8 > bit_tmp;
bit_tmp = m_input.status;
for (unsigned int i = 0; i < 2; i++)
{
out_msg.analog[i] = (float) m_input.analog[i]
* (float) EBOX_AIN_COUNTSTOVOLTS;
out_msg.encoder[i] = m_input.encoder[i];
out_msg.trigger[i] = bit_tmp[i];
}
bit_tmp = m_input.digital;
for (unsigned int i = 0; i < 8; i++)
out_msg.digital[i] = bit_tmp.test(i);
out_msg.timestamp = m_input.timestamp;
port_input.write(out_msg);
EBOXAnalog analog_msg;
if (port_output_analog.read(analog_msg) == NewData)
for (unsigned int i = 0; i < 2; i++)
writeAnalog(i, analog_msg.analog[i]);
EBOXDigital digital_msg;
if (port_output_digital.read(digital_msg) == NewData)
{
for (unsigned int i = 0; i < 8; i++)
bit_tmp.set(i, (digital_msg.digital[i] != 0));
m_output.digital = bit_tmp.to_ulong();
}
EBOXPWM pwm_msg;
if (port_output_pwm.read(pwm_msg) == NewData)
for (unsigned int i = 0; i < 2; i++)
writePWM(i, pwm_msg.pwm[i]);
*(out_eboxt*) (m_datap->outputs) = m_output;
}
