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; }