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