//forums.parallax.com/discussion/154274/the-artist-robot/p5
class TCruiseSpiral: public Node // Each task will be a class (derived from Node of course).
{
private:
uint32_t lastTimeExecuted;
float speedOuter;
float speedInner;
float waitUntilOuterCmReached;
float r;
float addRadius;
float segments;
float MAX_SPIRAL_RADIUS;
public:
TCruiseSpiral () {
segments = 16.0f; // How often should the calculation be done within 360 degrees. 16 Means every 22,5 degrees. This means that 360 is splited in 16 segments
addRadius = 17.0f/segments; // Radius that should be increased in one round divided by 16 because every 22° the speed and cm will calculated new. I chose 35.5/2=17 in because the cutter disc should overlap each round.
}
virtual void onInitialize(Blackboard& bb) {
r = 22.0f; //22.0f; // Start with bigger radius in order not to stall the inner wheel 35.5
calcSpeed(r,bb);
calcOuterCm(r);
bb.motor.startDistanceMeasurementSpiral();
bb.motor.L->setSpeed(speedOuter);
bb.motor.R->setSpeed(speedInner);
bb.driveDirection = DD_FORWARD;
MAX_SPIRAL_RADIUS = GETTF(TF_MAX_SPIRAL_RADIUS_CM);
}
void calcSpeed( float _r,Blackboard& bb) {
float d = GETTF(TF_DISTANCE_BETWEEN_WHEELS_CM);
speedOuter = mapf(_r, 22.0f, MAX_SPIRAL_RADIUS , bb.CRUISE_SPEED_MEDIUM, bb.CRUISE_SPEED_HIGH);
bb.cruiseSpeed= speedOuter;
speedInner= speedOuter * (2*_r-d) / (2*_r+d);
}
void calcOuterCm( float _r) {
float d = GETTF(TF_DISTANCE_BETWEEN_WHEELS_CM);
waitUntilOuterCmReached = (PI * (2*_r+d))/segments; // cm for 360 degree divided by 16 to get cm for 22,5 degrees
}
virtual NodeStatus onUpdate(Blackboard& bb) {
// get the driven distance: bb.motor.getDistanceLInCM() and check if it has driven the calculated way: waitUntilOuterCmReached
if (bb.motor.getDistanceLInCMSpiral() > waitUntilOuterCmReached) {
bb.motor.startDistanceMeasurementSpiral();
r+=addRadius;
calcSpeed(r,bb);
calcOuterCm(r);
bb.motor.L->setSpeed(speedOuter);
bb.motor.R->setSpeed(speedInner);
}
if(r > MAX_SPIRAL_RADIUS ) { // Stop Spiral after 150cm radius
return BH_SUCCESS;
}
return BH_RUNNING;
}
virtual void onTerminate(NodeStatus status, Blackboard& bb) {
bb.flagCruiseSpiral = false;
bb.lastTimeSpiralStarted = millis();
}
};
