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;
}
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;
}
int strategy_step() {
int next = 1;
struct wpt *goal;
while (next) {
printf("STRATEGY: %i %i %i\n",cycle_position, cycle[cycle_position], strategy_position);
next = 0;
goal = strat[cycle[cycle_position]] + strategy_position;
printf("use wpt %d,%d,%d,%d,%d\n", goal->x,goal->y,goal->rev,goal->unload,goal->sleep);
if (goal->unload) {
enc_type pos;
enc_get(&pos);
if (!((pos.pos_x > goal->x) || (pos.pos_y > goal->y))) {
do_servo(goal);
}
else {
u16 r,g,b,w;
char col='N';
i2c_taos_get_color();
usleep(1000*130);
if (i2c_taos_fetch_color(&r, &g, &b, &w) == ourcolor)
do_servo(goal);
}
next = 1;
}
else if ((goal->rev&3) == 0) {
i2c_taos_sort_enable();
motor_command(goal->x, goal->y, 0, 0, pos_error_allow);
}
else if ((goal->rev&3) == 1) {
i2c_taos_sort_enable();
motor_command(goal->x, goal->y, 1, 0, pos_error_allow);
}
else if ((goal->rev&3) == 2) {
i2c_taos_sort_disable();
motor_command(goal->x, goal->y, 0, 0, pos_error_allow);
}
else if ((goal->rev&3) == 3) {
i2c_taos_sort_disable();
motor_command(goal->x, goal->y, 1, 0, pos_error_allow);
}
strategy_position++;
if (strategy_position > strat[cycle[cycle_position]][0].x) {
strategy_position = 1;
cycle_position++;
if (cycle_position >= cycle_size)
cycle_position = 0;
}
}
printf("STRATEGY: done\n");
return (goal->rev >> 2)&1;
}
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_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_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;
}
int main(void) {
BOARD_Init();
printf("board_inited\n");
init_MDB();
AHRS_init();
printf("ahrs inited\n");
// timer_init();
//while(1);
int i = 700;
while (i >= -700) {
while (IsReceiveEmpty());
printf("looped\n");
printf("current angle: %f\n",get_AHRS_angle());
printf("current speed: %f\n", get_AHRS_rate());
waitabit(1000);
motor_command(i);
printf("sent command %d\n\n", i);
GetChar();
i = i - 100;
}
// while (1){
// while (IsReceiveEmpty());
// printf("looped\n");
// waitabit(1000);
// printf("current speed: %d\n", motor_command(10000));
// GetChar();
// }
}
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_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;
}
void __ISR(_TIMER_3_VECTOR, ipl3) Timer3Handler(void) {
mT3ClearIntFlag();
printf("timeout\n");
//printf("mspeed %d\n",motor_command(500));
//waitabit(1000);
//printf("angle %f\n",get_AHRS_angle());
//waitabit(1000);
// printf("speed %f\n", get_AHRS_rate());
//waitabit(500000); //works with 1000000 and 500000
//printf("mspeed %d\n\n", motor_command(500));
printf("duty cycle %d\n", PWM_GetDutyCycle(PWM_PORTY04));
motor_command(300);
//MDBSS ^= 1;
}
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;
}
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;
}
void test_fsm(u08 cmd, u08 *param)
{
//enum states { s_disabled=0, s_tracking_wall=1, s_lost_wall=2, s_turning_corner=3, s_turning_sharp_corner=4 };
static u08 state=0;
static u08 last_state=255;
static u32 t_start;
static s16 bias=0;
static u08 count;
u32 t_delta;
u08 i;
t_config_value v;
static u08 initialized=0;
if(!initialized)
{
initialized=1;
usb_printf("wall_follow_fsm()\n");
}
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
PUMP_ON();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
switch(state)
{
case 0:
t_start = get_ms();
motor_command(8,0,0,speed_profile[0].l_speed,speed_profile[0].r_speed);
state++;
break;
case 1:
t_delta = get_ms() - t_start;
i=0;
while(speed_profile[i].time < t_delta) i++;
motor_command(8,0,0,speed_profile[i-1].l_speed,speed_profile[i-1].r_speed);
break;
}
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
static s16 ne=0, nw=0;
ne = (ne + (s16) s.inputs.analog[AI_FLAME_NE])/2;
nw = (nw + (s16) s.inputs.analog[AI_FLAME_NW])/2;
bias = 0;
if( (ne>245) && (nw>245) ) bias = 0;
else if( abs(ne-nw) < 10 ) bias = 0;
else if( ne>nw ) bias = -1;
else if( nw>ne ) bias = 1;
v.u16 = cfg_get_u16_by_grp_id(15,6);
v.u16 += bias;
cfg_set_value_by_grp_id(15,6, v);
}
if(s.behavior_state[TEST_LOGIC_FSM]==6)
{
if(s.inputs.analog[AI_FLAME_NW]<80)
{
motor_command(6,3,3,-40,40);
}
else
{
motor_command(2,0,0,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
}
}
