ba_system_velocityController_1_.h 926 Bytes
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
#ifndef BA_SYSTEM_VELOCITYCONTROLLER_1_
#define BA_SYSTEM_VELOCITYCONTROLLER_1_
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
using namespace arma;
class ba_system_velocityController_1_{
public:
double holdTimeIn;
double maxVelIn;
double maxAccelIn;
double deltaTimeIn;
bool slowDownIn;
bool resetVelIn;
double curVelOut;
double lastVel;
double holdFor;
void init(double holdTimeIn)
{
21 22
this->holdTimeIn=holdTimeIn;

23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58
lastVel=0;
holdFor=0;
}
void execute()
{
double resVel = lastVel;
if(resetVelIn){
lastVel = maxVelIn;
resVel = maxVelIn;
holdFor = 0;
}
else {
if(slowDownIn){
resVel = lastVel-maxAccelIn*deltaTimeIn;
holdFor = holdTimeIn;
}
else {
holdFor = holdFor-deltaTimeIn;
if((holdFor <= 0)){
resVel = lastVel+maxAccelIn*(-holdFor);
holdFor = 0;
}
}
}
if((resVel < 0)){
resVel = 0;
}
else if((resVel > maxVelIn)){
resVel = maxVelIn;
}
curVelOut = resVel;
lastVel = curVelOut;
}

};
#endif