ER ev3_motor_sync(ID portA, ID portB, int speed, int turn_ratio) { ER ercd; lazy_initialize(); CHECK_PORT(portA); CHECK_PORT_CONN(portA); CHECK_PORT(portB); CHECK_PORT_CONN(portB); STEPSYNC ts; // DATA8 Cmd; // DATA8 Nos; // DATA8 Speed; // DATA16 Turn; // DATA32 Time; // DATA8 Brake; ts.Cmd = opOUTPUT_STEP_SYNC; ts.Nos = (1 << portA) | (1 << portB); ts.Speed = speed; ts.Turn = turn_ratio; ts.Step = 0; ts.Brake = false; motor_command(&ts, sizeof(ts)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_steer(motor_port_t left_motor, motor_port_t right_motor, int power, int turn_ratio) { ER ercd; // lazy_initialize(); CHECK_PORT(left_motor); CHECK_PORT_CONN(left_motor); CHECK_PORT(right_motor); CHECK_PORT_CONN(right_motor); // TODO: check if this is correct if (right_motor > left_motor) turn_ratio = turn_ratio * (-1); STEPSYNC ts; // DATA8 Cmd; // DATA8 Nos; // DATA8 Speed; // DATA16 Turn; // DATA32 Time; // DATA8 Brake; ts.Cmd = opOUTPUT_STEP_SYNC; ts.Nos = (1 << left_motor) | (1 << right_motor); ts.Speed = power; ts.Turn = turn_ratio; ts.Step = 0; ts.Brake = false; motor_command(&ts, sizeof(ts)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_steer(motor_port_t left_motor, motor_port_t right_motor, int power, int turn_ratio) { ER ercd; // lazy_initialize(); CHECK_PORT(left_motor); CHECK_PORT_CONN(left_motor); CHECK_PORT(right_motor); CHECK_PORT_CONN(right_motor); if (right_motor < left_motor) turn_ratio = turn_ratio * (-1); STEPSYNC ts; ts.Cmd = opOUTPUT_STEP_SYNC; ts.Nos = (1 << left_motor) | (1 << right_motor); ts.Speed = power; ts.Turn = turn_ratio * 2; ts.Step = 0; ts.Brake = false; motor_command(&ts, sizeof(ts)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_reset_counts(motor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); char buf[2]; #if 0 // TODO: check this buf[0] = opOUTPUT_RESET; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); #endif buf[0] = opOUTPUT_CLR_COUNT; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }
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_motor_reset_counts(motor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); char buf[2]; /** * Reset the counts when used as a motor. * Useful when the position of a motor is changed by hand. */ buf[0] = opOUTPUT_RESET; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); /** * Reset then counts when used as a tacho sensor. */ buf[0] = opOUTPUT_CLR_COUNT; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_rotate(motor_port_t port, int degrees, uint32_t speed_abs, bool_t blocking) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); // Float the motor first if it is busy if (*pMotorReadyStatus & (1 << port)) ev3_motor_stop(port, false); STEPSPEED ss; ss.Cmd = opOUTPUT_STEP_SPEED; ss.Speed = speed_abs * (degrees < 0 ? -1 : 1); ss.Step1 = 0; // Up to Speed ss.Step2 = (degrees < 0 ? -degrees : degrees); // Keep Speed ss.Step3 = 0; // Down to Speed ss.Brake = true; ss.Nos = 1 << port; motor_command(&ss, sizeof(ss)); if (blocking) // TODO: What if pMotorReadyStatus is kept busy by other tasks? while (*pMotorReadyStatus & (1 << port)); ercd = E_OK; error_exit: return ercd; }
ER ev3_sensor_config(sensor_port_t port, sensor_type_t type) { ER ercd; // lazy_initialize(); CHECK_PORT(port); sensors[port] = type; switch (type) { case NONE_SENSOR: case TOUCH_SENSOR: // Do nothing for analog sensor or no sensor break; case ULTRASONIC_SENSOR: case GYRO_SENSOR: case COLOR_SENSOR: // Configure UART sensor ercd = uart_sensor_config(port, 0); assert(ercd == E_OK); // Wait UART sensor to finish initialization // while(!uart_sensor_data_ready(port)); break; default: API_ERROR("Invalid sensor type %d", type); return E_PAR; } ercd = E_OK; error_exit: return ercd; }
ER_UINT ev3_sensor_get_type(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); return sensors[port]; error_exit: return ercd; }
HRESULT STDMETHODCALLTYPE CGainer::WriteAnalogOuput( /* [in] */ long Port, /* [in] */ long Value) { if (!m_iothread) return Error(IDS_E_NOTOPENED); CHECK_PORT(Port); char msg[8]; sprintf(msg, "a%d%02X*", Port, Value); notify_message(m_iothread_id, WM_COM_IO, msg); return S_OK; }
HRESULT STDMETHODCALLTYPE CGainer::ReadAnalogInput( /* [in] */ long Port, /* [retval][out] */ long *pResult) { if (!m_iothread) return Error(IDS_E_NOTOPENED); CHECK_PORT(Port); char msg[8]; printf(msg, "S%d*", Port); notify_message(m_iothread_id, WM_COM_IO, msg); return S_OK; }
HRESULT STDMETHODCALLTYPE CGainer::WriteDigitalOutput( /* [in] */ long Port, /* [in] */ VARIANT_BOOL High) { if (!m_iothread) return Error(IDS_E_NOTOPENED); CHECK_PORT(Port); char msg[8]; sprintf(msg, "%c%d*", (High) ? 'H' : 'L', Port); notify_message(m_iothread_id, WM_COM_IO, msg); return S_OK; }
ER_UINT ev3_motor_get_type(motor_port_t port) { ER ercd; CHECK_PORT(port); // lazy_initialize(); return mts[port]; error_exit: return ercd; }
bool_t ev3_touch_sensor_is_pressed(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == TOUCH_SENSOR, E_OBJ); int16_t val = analog_sensor_get_pin6(port); return val > (ADC_RES / 2); error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return false; }
int16_t ev3_gyro_sensor_get_rate(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == GYRO_SENSOR, E_OBJ); int16_t val; uart_sensor_fetch_data(port, GYRO_RATE, &val, sizeof(val)); return val; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return 0; }
uint8_t ev3_color_sensor_get_ambient(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == COLOR_SENSOR, E_OBJ); uint8_t val; uart_sensor_fetch_data(port, COL_AMBIENT, &val, sizeof(val)); return val; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return 0; }
colorid_t ev3_color_sensor_get_color(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == COLOR_SENSOR, E_OBJ); colorid_t val; uart_sensor_fetch_data(port, COL_COLOR, &val, sizeof(val)); assert(val >= COLOR_NONE && val < TNUM_COLOR); return val; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return COLOR_NONE; }
int ev3_motor_get_power(motor_port_t port) { // TODO: Should use ER & pointer instead ? ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); return *pMotorData[port].speed; error_exit: assert(ercd != E_OK); syslog(LOG_ERROR, "%s(): Failed to get motor power, ercd: %d", __FUNCTION__, ercd); return 0; }
ER ev3_gyro_sensor_reset(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == GYRO_SENSOR, E_OBJ); //uart_sensor_switch_mode(port, GYRO_CAL); uart_sensor_fetch_data(port, GYRO_CAL, NULL, 0); return E_OK; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return ercd; }
bool_t ev3_ultrasonic_sensor_listen(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == ULTRASONIC_SENSOR, E_OBJ); // TODO: TEST THIS API! uint8_t val; uart_sensor_fetch_data(port, US_LISTEN, &val, sizeof(val)); return val; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return false; }
ER ev3_motor_set_power(motor_port_t port, int power) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); motor_type_t mt = mts[port]; if (power < -100 || power > 100) { int old_power = power; if (old_power > 0) power = 100; else power = -100; syslog(LOG_WARNING, "%s(): power %d is out-of-range, %d is used instead.", __FUNCTION__, old_power, power); } char buf[3]; if (mt == UNREGULATED_MOTOR) { // Set unregulated power buf[0] = opOUTPUT_POWER; } else { // Set regulated speed buf[0] = opOUTPUT_SPEED; } buf[1] = 1 << port; buf[2] = power; motor_command(buf, sizeof(buf)); /** * Start the motor */ motor_command(buf, sizeof(buf)); buf[0] = opOUTPUT_START; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }
int16_t ev3_ultrasonic_sensor_get_distance(sensor_port_t port) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_COND(ev3_sensor_get_type(port) == ULTRASONIC_SENSOR, E_OBJ); #if 0 return ev3_uart_sensor_get_short(port) / 10; #endif int16_t val; uart_sensor_fetch_data(port, US_DIST_CM, &val, sizeof(val)); return val / 10; error_exit: syslog(LOG_WARNING, "%s(): ercd %d", __FUNCTION__, ercd); return 0; }
ER ev3_motor_stop(motor_port_t port, bool_t brake) { ER ercd; // lazy_initialize(); CHECK_PORT(port); CHECK_PORT_CONN(port); char buf[3]; buf[0] = opOUTPUT_STOP; buf[1] = 1 << port; buf[2] = brake; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_set_power(ID port, int power) { ER ercd; CHECK_PORT(port); CHECK_PORT_CONN(port); assert(power >= -100 && power <= 100); /* * Set power and start */ char buf[3]; buf[0] = opOUTPUT_POWER; buf[1] = 1 << port; buf[2] = power; motor_command(buf, sizeof(buf)); buf[0] = opOUTPUT_START; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }
ER ev3_motor_set_speed(ID port, int speed) { ER ercd; CHECK_PORT(port); CHECK_PORT_CONN(port); assert(speed >= -100 && speed <= 100); /* * Set speed and start */ char buf[3]; buf[0] = opOUTPUT_SPEED; buf[1] = 1 << port; buf[2] = speed; motor_command(buf, sizeof(buf)); buf[0] = opOUTPUT_START; buf[1] = 1 << port; motor_command(buf, sizeof(buf)); ercd = E_OK; error_exit: return ercd; }