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