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