int main(void) { uint32_t current_time = 0; SystemInit(); GpioInit(); //AdcInit(); TimerInit(0, 1ul * _millisecond); CallbackRegister(MotorHandler, 10ul * _millisecond); CallbackEnable(MotorHandler); TimerEnable(0); EncoderInit(); MotorInit(); MotorStart(); I2cInit(SENSOR_BUS); current_time = now; while(now < current_time + 1000); // center the wheels if they aren't already set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); current_time = now; while(now < current_time + 5000); // offset one motor set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); current_time = now; while(now < current_time + 1000); while (1) { if (get_ir_sen()) { set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER + 50, -70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, -72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); } else { set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); } } return 0; }
BOOL parse_position_command( char* mIncoming ) { char expression[] = "^motor position:([vV]\\d+)? ?([wW]\\d+)? ?([xX]\\d+)? ?([yY]\\d+)? ?([zZ]\\d+)?"; BOOL is_position_command = strcmp(mIncoming, "motor position "); byte which = which_motors(mIncoming); uint32_t pos; for (int b=0; b<NUM_MOTORS; b++) { if (is_in_set(which, b) ) set_motor_position( b, pos ); } form_response("ACK: position request"); return is_position_command; }
BOOL parse_limits_enable( char* mIncoming ) { char expression[] = "(enable|disable) limits"; BOOL is_position_command = strcmp(mIncoming, "enable limits"); is_position_command = strcmp(mIncoming, "disable limits"); byte which = which_motors(mIncoming); uint32_t pos; for (int b=0; b<NUM_MOTORS; b++) { if (is_in_set(which, b) ) set_motor_position( b, pos ); } form_response("ACK: limits enabled"); return is_position_command; }