void DataSet::calc() {
uint8_t* active = this->active.getPixels();
uint32_t* distance = this->distance.getPixels();
for (int i=0; i<this->size(); i++)
*active++ = *distance++ > distanceThreshold;
calcInverse();
}
int DeltaRobot::setCartesianPosition(float x, float y, float z, bool setSteppers) {
int ex = x;
int ey = y;
int ez = z+effectorHeadDepth;
float newTheta0, newTheta1, newTheta2;
int result = calcInverse(ex, ey, ez, newTheta0, newTheta1, newTheta2);
if (!positionIsPossible(x, y, z)) { //takes into account physical limitations
cout << "Position is not possible\n";
} else {
if (setSteppers) {
if (serialConnection.robotReadyForData()) {
float deltaD = distanceBetweenPoints(ofPoint(x, y, z), ofPoint(effectorX, effectorY, effectorZ)); //change in distance
float timeToMove = deltaD/unitSpeed; //in seconds
cout << "Delta D is "<<deltaD<<"\n";
cout << "Time to move is "<<timeToMove<<"\n";
float stepper0Speed = (100000*timeToMove)/(abs(newTheta0-theta0));
float stepper1Speed = (100000*timeToMove)/(abs(newTheta1-theta1));
float stepper2Speed = (100000*timeToMove)/(abs(newTheta2-theta2));
cout << "stepper0Speed:("<<stepper0Speed<<"), stepper1Speed:("<<stepper1Speed<<"), stepper2Speed("<<stepper2Speed<<")\n";
clock_t tStart = clock();
serialConnection.setStepper(0, newTheta0, 2000);
serialConnection.setStepper(1, newTheta1, 2000);
serialConnection.setStepper(2, newTheta2, 2000);
} else {
return -1; //dont move as steppers are not ready
}
}
theta0 = newTheta0;
theta1 = newTheta1;
theta2 = newTheta2;
effectorX = ex;
effectorY = ey;
effectorZ = ez;
}
// cout << "input value is ("<<x<<","<<y<<","<<z<<")\n";
// cout << "theta 0 has the angle "<<theta0<<"\n";
// cout << "theta 1 has the angle "<<theta1<<"\n";
// cout << "theta 2 has the angle "<<theta2<<"\n";
// cout << "result is :"<<result<<"\n";
return result;
}
bool DeltaRobot::positionIsPossible(float x0, float y0, float z0) {
float a, b, c;
//cout << "a: " << a << " b: " << b << " c: " << c << "|| x0: " << x0 << " y0: " << y0 << " z0: " << z0 << "\n""\n";
//cout << "calc inverse = " << calcInverse(x0, y0, x0, a, b, c) <<"\n";
if (calcInverse(x0, y0, z0+effectorHeadDepth, a, b, c)==0) { //kinematically possible
if ((a>-18)&&(a<72)&&(b>-18)&&(b<72)&&(c>-18)&&(c<72)) { //within physical limitations ###needs confirmation
return true;
}
}
return false;
}
