void DeltaRobot::gotoNextWaypt() {
if (serialConnection.robotReadyForData()) {
ofPoint nextWaypt = queuedWaypoints[0];
setCartesianPosition(nextWaypt.x, nextWaypt.y, nextWaypt.z, true);
cout <<"set steppers to (t0:"<<theta0<<", t1:"<<theta1<<", t2:"<<theta2<<")\n";
// cout <<"Waypoints to go: "<<queuedWaypoints.size()<<"\n";
queuedWaypoints.erase(queuedWaypoints.begin()); //pop waypt off the front
cout <<"Waypoints to go: "<<queuedWaypoints.size()<<"\n";
cout << "effector position should be at ("<<nextWaypt.x<<","<<nextWaypt.y<<","<<nextWaypt.z<<"). It is at ("<<effectorX<<","<<effectorY<<","<<effectorZ<<")\n";
} else {
// cout <<"there is a waypoint (wayps left:"<<queuedWaypoints.size()<<") in queue, but robot is busy\n";
}
}
DeltaRobot::DeltaRobot(float ieffectorSideLength) { //constructor
//effector side is 320mm
// baseSideMultiplier = 1.471; //500mm
// upperArmMultiplier = 2.352; //700mm
// lowerArmMultiplier = 3.235; //1100mm
//
effectorSideLength = ieffectorSideLength;
// baseSideLength = effectorSideLength*baseSideMultiplier;
// upperArmLength = effectorSideLength*upperArmMultiplier;
// lowerArmLength = effectorSideLength*lowerArmMultiplier;
baseSideLength = 500;
upperArmLength = 700;
lowerArmLength = 1100;
kinematics = new Kinematics(effectorSideLength, baseSideLength, lowerArmLength, upperArmLength);
baseSideMultiplier = baseSideLength/effectorSideLength;
upperArmMultiplier = upperArmLength/effectorSideLength;
lowerArmMultiplier = lowerArmLength/effectorSideLength;
effectorHeadDepth = 200; //distance from plane of wrist axis to dot of laser
unitSpeed = 500; // mm/s
serialConnection.setupDevices();
setCartesianPosition(0, 0, 1000, false);
// setCartesianPosition(0, 0, -100, true);
//calculateWorkingPointCloud(); //calculate point cloud straight away
cout << "Delta Robot class instantiated \n";
}
void setCartesianPosition(const Vector3D& v){
setCartesianPosition(v.asMutableVector3D());
}
