ER ev3_motor_config(motor_port_t port, motor_type_t type) {
ER ercd;
CHECK_PORT(port);
CHECK_MOTOR_TYPE(type);
// lazy_initialize();
mts[port] = type;
/*
* Set Motor Type
*/
char buf[TNUM_MOTOR_PORT + 1];
buf[0] = opOUTPUT_SET_TYPE;
for (int i = EV3_PORT_A; i < TNUM_MOTOR_PORT; ++i)
buf[i + 1] = getDevType(mts[i]);
motor_command(buf, sizeof(buf));
/*
* Set initial state to IDLE
*/
buf[0] = opOUTPUT_STOP;
buf[1] = 1 << port;
buf[2] = 0;
motor_command(buf, sizeof(buf));
ercd = E_OK;
error_exit:
return ercd;
}
ER ev3_motors_init(motor_type_t typeA, motor_type_t typeB, motor_type_t typeC, motor_type_t typeD) {
ER ercd;
CHECK_MOTOR_TYPE(typeA);
CHECK_MOTOR_TYPE(typeB);
CHECK_MOTOR_TYPE(typeC);
CHECK_MOTOR_TYPE(typeD);
lazy_initialize();
mts[EV3_PORT_A] = typeA;
mts[EV3_PORT_B] = typeB;
mts[EV3_PORT_C] = typeC;
mts[EV3_PORT_D] = typeD;
/**
* Set device types
*/
char buf[TNUM_MOTOR_PORT + 1];
buf[0] = opOUTPUT_SET_TYPE;
for (int i = EV3_PORT_A; i < TNUM_MOTOR_PORT; ++i)
buf[i + 1] = getDevType(mts[i]);
ercd = motor_command(buf, sizeof(buf));
assert(ercd == E_OK);
/**
* Set initial state to IDLE
*/
buf[0] = opOUTPUT_STOP;
buf[1] = 0xF;
buf[2] = 0;
ercd = motor_command(buf, sizeof(buf));
assert(ercd == E_OK);
ercd = E_OK;
error_exit:
return ercd;
}
