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