void PathPlanner::setInitPos(AxisVector initPos) {
AxisVector z; z.zero();
std::array<AxisVector, 3> r;
r[0] = initPos;
r[1] = z;
r[2] = z;
trajectoryGen.reset(r);
lastPoint = initPos;
}
void PathPlanner::setInitPos(AxisVector initPos) {
AxisVector z; z.zero();
// std::array<AxisVector, 4> r;
std::vector<AxisVector> r = std::vector<AxisVector>(4);
r[0] = initPos;
r[1] = z;
r[2] = z;
r[3] = z;
this->last = r;
this->finish = true;
}
// bool PathPlanner::move(std::array<AxisVector, 100> array, bool limitOn) {
bool PathPlanner::move(std::vector<AxisVector> array, bool limitOn) {
if(!finish)
return false;
coefficients.zero();
positions = array;
jerk_limit_on = limitOn;
// find end of array
AxisVector zero; zero.zero();
int nr_in_points = 0;
for(int i = 0; i < array.size(); i++){
if(array[i] < zero)
i = array.size();
else
nr_in_points++;
}
// Safety check, if -1 at end of array missing, don't calculate trajectory
// if(nr_in_points == array.size()){
// std::cout << "return false" << std::endl;
// return false;
// }
this->points_nr = nr_in_points;
if(calculateCoefficients_fromPosition()){
finish = false;
finish_segment = false;
t = 0;
dTold = 0;
k = 0;
}
else {
finish = true;
finish_segment = true;
t = 0;
dTold = 0;
k = 0;
}
return true;
}
inline AxisValue getAxis(unsigned int axis) const {
if (axisState.size() <= axis) {
return AxisValue::null();
}
return axisState[axis];
}
