Beispiel #1
0
void DoAuto(void){
	int tmp = 0;
	int b = 0;
	int i = 0;
	tmp = RefPos();
	if(!tmp){
		MotorStop();
		return;
	}
	MotorStart();
	for(;i<20;i++){
		b = borr_auto[i];
		//if
		tmp = Nstep(b);
		if(!tmp){
			MotorStop();
			return;
		}
		tmp = DrillHole();
		if(!tmp){
			MotorStop();
			return;
		}		
	}
	MotorStop();
	return;
}
Beispiel #2
0
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;
}
Beispiel #3
0
static void sysEventHandle(uint8_t event, void *args)
{
    MProtoStepInfo_t *stepInfo = NULL;
    
    switch(event)
    {
    case SYS_EVENT_SELFCHECK:
        break;
    case SYS_EVENT_MOTOR_CONTRL:
        stepInfo = (MProtoStepInfo_t *)args;
        g_curStepInfo = stepInfo->step;
        g_stepCount = stepInfo->count - 1;
        g_sysStatus = SYS_STATUS_BUSY;
        MotorStart(g_curStepInfo->id, g_curStepInfo->dir, g_curStepInfo->cycle);
        break;
    default:
        break;
    }
}
Beispiel #4
0
void DoAuto(void){
	int i = 0;
	if(!RefPos()){
		MotorStop();
		return;
	}
	MotorStart();
	while(pattern[i] != 0xFF){
		if(!Nstep(pattern[i])){
			MotorStop();
			return;
		}
		if(!DrillHole()){
			MotorStop();
			return;
		}
		i++;
	}
	MotorStop();
	return;
}
Beispiel #5
0
static void sysMotorEventHandle(uint8_t id, MotorEvent_t event)
{
    bool doNextStep = false;

    if(MOTOR_EVENT_STEP_OVER == event)
    {
        doNextStep = true;
        if(g_curStepInfo->flag == 1) 
        {
            MProtoCtrlResult(MPROTO_RESULT_SENOR_ERROR); //´«¸ÐÆ÷δ´¥·¢
        }
    }
    else if(MOTOR_EVENT_SENSOR_TRIGGERED == event)
    {
        if(g_curStepInfo != NULL 
            && g_curStepInfo->id == id
            && g_curStepInfo->flag == 1)
        {
            doNextStep = true;
        }
    }

    if(doNextStep)
    {
        MotorStop(id);
        if(g_stepCount == 0)
        {
            g_sysStatus = SYS_STATUS_IDLE;
            MProtoCtrlResult(MPROTO_RESULT_SUCCESS);
            g_curStepInfo = NULL;
        }
        else
        {
            g_curStepInfo++;
            g_stepCount--;
            MotorStart(g_curStepInfo->id, g_curStepInfo->dir, g_curStepInfo->cycle);
        }
    }
}
Beispiel #6
0
void DoAuto(){
	int pattern[21] = {0,1,1,1,1,1,1,1,2,1,5,2,2,2,2,4,4,3,8,2,255};
	int antalSteg;
	int subrutinRes;
	
	if(RefPos()){	
		int i=0;
		MotorStart();
		while(1){
		antalSteg = pattern[i];
		i++;
		if(antalSteg == 255)
			break;
		subrutinRes = Nstep(antalSteg);
		if(subrutinRes == 0)
			break;
		subrutinRes = DrillHole();
		if(subrutinRes == 0)
			break;
		}
	}
	MotorStop();
}