bool jointData::write(const yarp::os::idl::WireWriter& writer) const {
if (!write_jointPosition(writer)) return false;
if (!write_jointPosition_isValid(writer)) return false;
if (!write_jointVelocity(writer)) return false;
if (!write_jointVelocity_isValid(writer)) return false;
if (!write_jointAcceleration(writer)) return false;
if (!write_jointAcceleration_isValid(writer)) return false;
if (!write_motorPosition(writer)) return false;
if (!write_motorPosition_isValid(writer)) return false;
if (!write_motorVelocity(writer)) return false;
if (!write_motorVelocity_isValid(writer)) return false;
if (!write_motorAcceleration(writer)) return false;
if (!write_motorAcceleration_isValid(writer)) return false;
if (!write_torque(writer)) return false;
if (!write_torque_isValid(writer)) return false;
if (!write_pwmDutycycle(writer)) return false;
if (!write_pwmDutycycle_isValid(writer)) return false;
if (!write_current(writer)) return false;
if (!write_current_isValid(writer)) return false;
if (!write_controlMode(writer)) return false;
if (!write_controlMode_isValid(writer)) return false;
if (!write_interactionMode(writer)) return false;
if (!write_interactionMode_isValid(writer)) return false;
return !writer.isError();
}
int main(void)
{
/*init all the things*/
motors_init(PHASE_CORRECT, PRESCALE_8);
uart_init(BAUD_CALC(9600));
sei();
adc_init();
wdt_init(WDTO_500MS);
/********************/
go(0, 0);
uart_putc(ACK);
over_current_a = 0;
over_current_b = 0;
while (1)
{
current_a = analog_read(C_SENS_A);
current_b = analog_read(C_SENS_B);
req_t = get_request_type();
if (req_t == CURRENT_REQ)
{
write_current(current_a, current_b);
}
else if (req_t == MOTORS_SET)
{
read_motors(&speed_a, &speed_b);
wdt_reset();
}
//manage current sens
#ifdef CURRENT_LIMIT
if ((current_a < CURRENT_LIMIT) && (current_b < CURRENT_LIMIT))
{
go(speed_a, speed_b);
over_current_a = over_current_b = 0;
}
else
{
//overcurrent
go(0, 0);
uart_putc('!');
over_current_a = current_a - CURRENT_LIMIT;
over_current_b = current_b - CURRENT_LIMIT;
}
#endif
}
return (0);
}
