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