Helvijs Kiselis
/
Micromouse
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Motion.cpp
- Committer:
- Helvis
- Date:
- 2018-05-15
- Revision:
- 24:11c5fb5280eb
- Parent:
- 23:accd07ca2da7
- Child:
- 25:8854037f6336
File content as of revision 24:11c5fb5280eb:
#include <cmath>
#include "Motion.h"
using namespace std;
const float Motion::LEFT_MID_VAL = 40.73f; //44.73
const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
const float Motion::KP = 2.5;
const float Motion::KD = 0.0f;
const int Motion::MOVE_DIST = 1605;
const float Motion::MOVE_SPEED = 50.0f;
const float Motion::SCAN_SPEED = 50.0f;
const float Motion::ROTATE_SPEED = 80.0f;
const float Motion::ACCEL_CONST = 3.5f; //2.212f
Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
EncoderCounter& counterRight, IRSensor& irSensorL,
IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
DigitalOut& enableMotorDriver) :
controller(controller), counterLeft(counterLeft),
counterRight(counterRight), irSensorL(irSensorL),
irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
enableMotorDriver(enableMotorDriver) {}
Motion::~Motion() {}
/**
* Eine Feldstrecke druchführen
*/
void Motion::move() {
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
t.start();
while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
countsL = counterLeft.read();
countsR = counterRight.read();
distanceC = irSensorC.readC();
distanceL = irSensorL.readL();
distanceR = irSensorR.readR();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
accel(MOVE_SPEED);
control();
if ((distanceC) < 40.0f) {
countsLOld = countsL;
countsROld = countsR;
while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
countsL = counterLeft.read();
countsR = counterRight.read();
distanceC = irSensorC.readC();
if (distanceC > 40.0f) {
stop();
break;
}
}
stop();
break;
}else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
countsLOld += 500;
countsROld += 500;
}/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
countsLOld = counterLeft.read();
countsROld = counterRight.read();
while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) {
countsL = counterLeft.read();
countsR = counterRight.read();
accel(MOVE_SPEED);
control();
}
stop();
break;
}*/
}
t.stop();
t.reset();
lastMove = false;
}
/**
* Eine Feldstrecke druchführen
*/
void Motion::moveHalf() {
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
t.start();
while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
countsL = counterLeft.read();
countsR = counterRight.read();
distanceC = irSensorC.readC();
distanceL = irSensorL.readL();
distanceR = irSensorR.readR();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
accel(MOVE_SPEED);
control();
if (distanceC < 101.0f && lastMove == false) {
stop();
break;
}else if (distanceC < 40.0f && lastMove == true) {
stop();
break;
}
}
t.stop();
t.reset();
lastMove = false;
}
/**
* Eine Feldstrecke mit überprüfung der Ziellinie fahren
*/
void Motion::scanMove() {
acceleration = false;
deceleration = false;
longMove = false;
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
t.start();
while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
countsL = counterLeft.read();
countsR = counterRight.read();
distanceC = irSensorC.readC();
distanceL = irSensorL.readL();
distanceR = irSensorR.readR();
if (enableMotorDriver == 0) {
enableMotorDriver = 1;
}
if (lineSensor.read() > 0.85f) {
line = 1;
}
accel(SCAN_SPEED);
control();
if ((distanceC) < 40.0f) {
countsLOld = countsL;
countsROld = countsR;
while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
countsL = counterLeft.read();
countsR = counterRight.read();
if (distanceC > 40.0f) {
stop();
break;
}
}
stop();
break;
}else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
countsLOld += 500;
countsROld += 500;
}
}
t.stop();
t.reset();
lastMove = false;
}
/**
* 90° Rotation nach Links
*/
void Motion::rotateL() {
stop();
controller.counterReset();
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
controller.setDesiredSpeedLeft(-ROTATE_SPEED);
controller.setDesiredSpeedRight(-ROTATE_SPEED);
while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) {
//accel();
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
stop();
}
/**
* 90° Rotation nach Rechts
*/
void Motion::rotateR() {
stop();
controller.counterReset();
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
controller.setDesiredSpeedLeft(ROTATE_SPEED);
controller.setDesiredSpeedRight(ROTATE_SPEED);
while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) {
//accel();
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
stop();
}
/**
* Links abbiegen
*/
void Motion::turnL() {
controller.counterReset();
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
controller.setDesiredSpeedLeft(17.0f);
controller.setDesiredSpeedRight(-83.0f);
while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
}
/**
* Rechts abbiegen
*/
void Motion::turnR() {
controller.counterReset();
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
controller.setDesiredSpeedLeft(83.0f);
controller.setDesiredSpeedRight(-17.0f);
while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
}
/**
* Motor Stop
*/
void Motion::stop() {
controller.setDesiredSpeedLeft(0.0f);
controller.setDesiredSpeedRight(0.0f);
actSpeed = 0.0f;
float sL = controller.getSpeedLeft();
float ticks = 0.08f*sL;
waitStop = 0;
while( waitStop < ticks) {
controller.setDesiredSpeedLeft(0.0f);
controller.setDesiredSpeedRight(0.0f);
waitStop+= 1;
}
}
/**
* 180° Rotation
*/
void Motion::rotate180() {
//1723
stop();
controller.counterReset();
countsLOld = counterLeft.read();
countsROld = counterRight.read();
countsL = counterLeft.read();
countsR = counterRight.read();
//controller.setDesiredSpeedLeft(-ROTATE_SPEED);
//controller.setDesiredSpeedRight(-ROTATE_SPEED);
t.start();
while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) {
actSpeed = 3.5f * t.read()*60.0f;
controller.setDesiredSpeedLeft(-actSpeed);
controller.setDesiredSpeedRight(-actSpeed);
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
t.reset();
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
while ((countsL - countsLOld) > -1720 || (countsR - countsROld) > -1720) {
actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
controller.setDesiredSpeedLeft(-actSpeed);
controller.setDesiredSpeedRight(-actSpeed);
countsL = counterLeft.read();
countsR = counterRight.read();
if (enableMotorDriver == 0) {enableMotorDriver = 1;}
}
t.stop();
t.reset();
stop();
}
void Motion::control() {
float wallLeft = 47.0f; //48.73
float wallRight = 44.0f; //51.03
distanceL = irSensorL.readL();
distanceR = irSensorR.readR();
if (distanceL < wallLeft && distanceR < wallRight) {
errorP = distanceL - distanceR - 3.0f;
}/* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
errorP = distanceL - LEFT_MID_VAL;
}else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
errorP = RIGHT_MID_VAL - distanceR;
}*/else if (distanceL < wallLeft && distanceR > wallRight) {
errorP = distanceL - wallLeft;
}else if (distanceL < wallLeft+15.0f && distanceL > wallLeft && distanceR > wallRight) {
errorP = distanceL - wallLeft;
}else if (distanceL > wallLeft && distanceR < wallRight) {
errorP = wallRight - distanceR;
}else if (distanceR < wallRight+15.0f && distanceL > wallLeft && distanceR > wallRight) {
errorP = wallRight - distanceR;
}else{
errorP = 0;
errorD = 0;
}
errorD = errorP - oldErrorP;
oldErrorP = errorP;
if (abs(errorP) < 80.0f) {
totalError = KP*errorP + KD*errorD;
}/*else{
totalError = 0;
}*/
controller.setDesiredSpeedLeft(actSpeed - totalError);
controller.setDesiredSpeedRight(-actSpeed - totalError);
}
void Motion::runTask(int path[],int task, bool reverse, int junction) {
switch(path[task]) {
case 1:
//Acceleration
if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
acceleration = true;
longMove = true;
deceleration = false;
}else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) {
acceleration = true;
longMove = true;
deceleration = false;
}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) {
deceleration = true;
acceleration = false;
lastMove = true;
longMove = false;
}else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
deceleration = true;
acceleration = false;
lastMove = true;
longMove = false;
}else{
deceleration = false;
}
//printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
//printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
move();
break;
case 2:
rotateL();
break;
case 3:
rotateR();
break;
case 4:
if (reverse == false && path[task] == 4 && path[task+1] == 1) {
acceleration = true;
longMove = true;
deceleration = false;
}else{
acceleration = false;
deceleration = false;
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
}
if (reverse == false && path[task-1] == 1 && path[task] == 4) {
deceleration = true;
acceleration = false;
lastMove = true;
longMove = false;
}else{
deceleration = false;
}
if (reverse == false && path[task+1] == 0) {
lastMove = true;
}
moveHalf();
break;
case 5:
turnL();
break;
case 6:
turnR();
break;
case 7:
break;
}
}
int Motion::finish() {
return line;
}
void Motion::accel(float targetSpeed) {
avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
if ( avgSpeed < targetSpeed && deceleration == false && acceleration == false) {
actSpeed = ACCEL_CONST * t.read()*60.0f;
}else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
actSpeed = 50.0f;
}else if (avgSpeed < targetSpeed*2.8f && acceleration == true) {
actSpeed = ACCEL_CONST * t.read()*60.0f;
}else if (avgSpeed > targetSpeed && deceleration == true) {
actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f;
}else if (avgSpeed < targetSpeed && deceleration == true) {
actSpeed = 50.0f;
}
}
