Diff: Motion.cpp
- Revision:
- 30:bdb8c92434a0
- Parent:
- 28:b7ce1e3bf08b
- Child:
- 31:2c54f8304ef5
--- a/Motion.cpp Thu May 17 16:39:03 2018 +0000
+++ b/Motion.cpp Tue May 22 16:40:36 2018 +0000
@@ -8,11 +8,11 @@
const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
const float Motion::KP = 2.0;
const float Motion::KD = 0.0f;
-const int Motion::MOVE_DIST = 1605;
+const int Motion::MOVE_DIST = 1620;
const float Motion::MOVE_SPEED = 50.0f;
const float Motion::RUN_SPEED = 70.0f;
const float Motion::ROTATE_SPEED = 80.0f;
-const float Motion::ACCEL_CONST = 3.5f; //2.212f
+const float Motion::ACCEL_CONST = 4.0f; //2.212f
Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
@@ -56,6 +56,7 @@
else accel(MOVE_SPEED);
control();
+ //Stop at certainn distance before wall
if ((distanceC) < 40.0f && speedRun == false) {
countsLOld = countsL;
countsROld = countsR;
@@ -64,6 +65,11 @@
countsL = counterLeft.read();
countsR = counterRight.read();
distanceC = irSensorC.readC();
+
+ if (speedRun == true) accel(RUN_SPEED);
+ else accel(MOVE_SPEED);
+ control();
+
if (distanceC > 40.0f) {
stop();
break;
@@ -71,27 +77,33 @@
}
stop();
break;
-
- }else if (distanceC < 101.0f && speedRun == true) {
+
+ //Stop at certain distance in speed run
+ }else if (distanceC < 160.0f && speedRun == true && lastMove == true) {
stop();
break;
+
+ //Correct distance from wall
}else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
countsLOld += 500;
- countsROld += 500;
+ countsROld -= 500;
- }/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
+ //Stop after certain distance if side wall and front wall missing
+ }else if ( deceleration == true && speedRun == false && ( distanceL > 60.0f || distanceR > 60.0f ) && distanceC > 200.0f ) {
countsLOld = counterLeft.read();
countsROld = counterRight.read();
- while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) {
+ while ((countsL - countsLOld) < MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
countsL = counterLeft.read();
countsR = counterRight.read();
- accel(MOVE_SPEED);
+
+ if (speedRun == true) accel(RUN_SPEED);
+ else accel(MOVE_SPEED);
control();
}
stop();
break;
- }*/
+ }
}
@@ -129,8 +141,25 @@
accel(RUN_SPEED);
control();
- if (distanceC < 101.0f && lastMove == false) {
+ //Stop ceratin distance before wall
+ if (distanceC < 100.0f && lastMove == false) {
break;
+
+ //Stop after certain distance if side wall missing
+ }else if ( (distanceL > 60.0f || distanceR > 60.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) {
+ countsLOld = counterLeft.read();
+ countsROld = counterRight.read();
+
+ while ((countsL - countsLOld) < 600.0f || (countsR - countsROld) > -600.0f) {
+ countsL = counterLeft.read();
+ countsR = counterRight.read();
+ accel(RUN_SPEED);
+ control();
+ }
+ stop();
+ break;
+
+ //Stop before wall at target field
}else if (distanceC < 40.0f && lastMove == true) {
stop();
break;
@@ -406,7 +435,9 @@
errorP = RIGHT_MID_VAL - distanceR;
- }*/ if (distanceL < wallLeft && distanceR > wallRight) {
+ }*/
+
+ if (distanceL < wallLeft && distanceR > wallRight) {
errorP = distanceL - wallLeft;
@@ -443,7 +474,8 @@
}
void Motion::runTask(int path[],int task, bool reverse, int junction) {
-
+
+ //reverse happens only in search run
if (reverse == false) speedRun = true;
else speedRun = false;
@@ -452,13 +484,13 @@
case 1:
//Acceleration
- if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
+ if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { //if next move() and previous no move() and step no junction
acceleration = true;
longMove = true;
deceleration = false;
- }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) {
+ }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0) ) { //same as above, also if start field
acceleration = true;
longMove = true;
@@ -466,25 +498,28 @@
}else{
acceleration = false;
- deceleration = false;
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
}
//Deceleration
- if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {
+ if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() or junction and speed above 120rpm
deceleration = true;
acceleration = false;
lastMove = true;
longMove = false;
- }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) {
+ }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() and no moveHalf() and speed above 120rpm
deceleration = true;
acceleration = false;
lastMove = true;
longMove = false;
+ }else if (reverse == false && path[task+1] != path[task] && path[task+1] == 4 && avgSpeed > 2.4f*MOVE_SPEED) {
+
+ lastMove = true;
+
}else{
deceleration = false;
}
@@ -502,7 +537,7 @@
break;
case 4:
- if (reverse == false && path[task] == 4 && path[task+1] == 1) {
+ if (reverse == false && path[task] == 4 && path[task+1] == 1) { //accelerate if next step is move()
acceleration = true;
longMove = true;
@@ -510,15 +545,13 @@
}else{
acceleration = false;
- deceleration = false;
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
}
- if (reverse == false && path[task-1] == 1 && path[task] == 4) {
+ if (reverse == false && path[task-1] == 1 && path[task] == 4 && path[task+1] != 0) { //decelerate if previous step was move()
deceleration = true;
acceleration = false;
- lastMove = true;
longMove = false;
}else{
@@ -530,7 +563,6 @@
lastMove = true;
}
-
moveHalf();
break;
case 5:
Helvijs Kiselis
