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