예제 #1
0
void adjustDirection() {
    //IR sensor make robot shake
    int l, r;
    for (int i = 0; i < 350; i++) {
        l = shortIR_LF.getDis();
        r = shortIR_RF.getDis();
        //delay(1);
        if (r > l) {
            md.setSpeeds(-adjustDirectionSpeed, adjustDirectionSpeed);
        } else if (r < l) {
            md.setSpeeds(adjustDirectionSpeed, -adjustDirectionSpeed);
        }
    }
    brakeForRotation();
}
예제 #2
0
void goAhead(int grids) {
    MODE = 1;
    delta = 0;
    currentSpeed = speedModeSpeed;
    if (grids == 1) {
        rightMCtr = leftMCtr = RisingEdgePerGrid_300;
    } else {
        MODE = 2;
        currentSpeed = fastRunSpeed;
        rightMCtr = leftMCtr = RisingEdgeForSP * grids;
    }

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    md.init();
    // md.setM2Speed(currentSpeed);
    // delay(4);
    // md.setM1Speed(currentSpeed);
    md.setSpeeds(currentSpeed, currentSpeed);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    brakeForGoAhead();
    
    //update direciton
    switch (pwd) {
        case 1: currentY -=grids;

                if (currentY < 2) {
                    currentY = 2;
                }
                break;
        case 2: currentX +=grids;
                if (currentX > 19) {
                    currentX = 19;
                }
                break;
        case 3: currentY +=grids;
                if (currentY > 14) {
                    currentY = 14;
                }
                break;
        case 4: currentX -=grids;
                if (currentX < 2) {
                    currentX = 2;
                }
                break;
        default: break;
    }
    //if (MODE != 2) {
        delay(50);
    //}
}
void loop() {
    int grid = 3;
    delta = 0;
    timer = 0;
    rightMCtr = leftMCtr = 400 * grid;

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    md.init();
    md.setSpeeds(300, 300);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    md.brakeWithABS();
    Serial.println(delta);
    Serial.println(timer);
    delay(3000);
}
예제 #4
0
void parking_U() {
    int u_F_dis = u_F.getDis();
    while (u_F_dis > 10) {
        md.setSpeeds(190, 190);
        delay(3);
        u_F_dis = u_F.getDis();
    }
    brakeForRotation();
}
예제 #5
0
void adjustDistance() {
    //IR sensor correct position
    int l, r, frontDis;
    for (int i = 0; i < 1200; i++) {
        l = shortIR_LF.getDis();
        r = shortIR_RF.getDis();
        delay(7);
        frontDis = max(l, r);
        
        if (frontDis < 505) {
            md.setSpeeds(adjustDistanceSpeed, adjustDistanceSpeed);
        } else if (frontDis > 510) {
            md.setSpeeds(-adjustDistanceSpeed, -adjustDistanceSpeed);
        } else {
            break;
        }
    }
    brakeForRotation();
}
예제 #6
0
void parking() {
    int ir_dis = min(shortSensorToCM(shortIR_LF.getDis()), shortSensorToCM(shortIR_RF.getDis()));
    Serial.println(ir_dis);
    while (ir_dis > 15) {
        md.setSpeeds(190, 190);
        delay(3);
        ir_dis = min(shortSensorToCM(shortIR_LF.getDis()), shortSensorToCM(shortIR_RF.getDis()));
        Serial.println(ir_dis);
    }
    brakeForRotation();
}
예제 #7
0
void turn(int turnRight) {
    MODE = 0;
    direction = turnRight;
    delta = 0;
    if (turnRight == 1) {
        rightMCtr = leftMCtr = RisingEdgePerTurnRight_200;
    } else {
        rightMCtr = leftMCtr = RisingEdgePerTurnLeft_200;
    }

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    //sp.setSpeedLvls(3, 3);
    md.init();
    md.setSpeeds(-200 * direction, 200 * direction);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    brakeForRotation();
    
    //update direction
    pwd += turnRight;
    if (pwd == 5) {
        pwd = 1;
    } else if (pwd == 0) {
        pwd = 4;
    }
    delay(30);
}