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