// ---------------------------------------------------------------------------------
// It sets the targetSpeed[] of the robot according to the speed profile parameters.
// This is called from a regular interrupt.
// Both the Xspeed and Wspeed profile run in parallel
//
// The profile generator keeps checking the distance left to travel with the
// curAcc[]. When it's time to decelerate, it will set the targetSpeed[] to the
// targetEndSpeed[]
// ---------------------------------------------------------------------------------
void DoMoveCommand(void) {
int i;
unsigned int decelerationRequired;
for (i = 0; i < NUM_OF_SPEED; i++) {
if (moveState[i] == -1) // No active profile
continue;
decelerationRequired = GetDecRequired(finalPos[i]-curPos[i], afterBrakeDist[i], curSpeed[i], targetEndSpeed[i]);
if (decelerationRequired >= ABS(curAcc[i])) {
// Time to decelerate
#ifndef move
if (moveState[i] == 0) {
moveState[i] = 1;
targetSpeed[i] = targetEndSpeed[i];
if (targetSpeed[i] == 0) {
// when targetSpeed[i] == 0, the robot might not
// be able to reach the final position due to
// truncation error.
// solution is to temporarily set the speed slightly
// more than zero.
targetSpeed[i] = bDistDirFlag[i] ? curAcc[i]: -curAcc[i];
}
}
#else
moveState[i] = 1;
targetSpeed[i] = targetEndSpeed[i];
#endif
// set curAcc[i] to decelerationRequired else the robot
// might not be able to reach targetSpeed[i] when the
// target distance is reached. The result is that the robot
// might need to stop at a high speed which caused it to
// jerk.
// Changing the curAcc[i] here means that we do not know what's
// the curAcc[i] after a speed profile is completed and
// we need to set curAcc[i] again. If you do not want
// curAcc[i] to change, comment off the following line.
curAcc[i] = decelerationRequired;
}
if (bDistDirFlag[i]) {
// distance is towards positive direction
if (curPos[i] >= finalPos[i]) {
moveState[i] = -1;
curSpeed[i] = targetSpeed[i] = targetEndSpeed[i];
}
}
else {
// distance is towards negative direction
if (curPos[i] <= finalPos[i]) {
moveState[i] = -1;
curSpeed[i] = targetSpeed[i] = targetEndSpeed[i];
}
}
}
}
// ---------------------------------------------------------------------------------
// It set the targetSpeed[] of the robot according to the speed profile parameters.
// This is called from a regular interrupt.
// Both the Xspeed and Wspeed profile run in parallel
//
// The profile generator keeps checking the distance left to travel with the
// curAcc[]. When it's time to decelerate, it will set the targetSpeed[]to the
// targetEndSpeed[]
// ---------------------------------------------------------------------------------
void DoMoveCommand( ) {
int i;
float decelerationRequired;
for (i=0; i<NUM_OF_SPEED; i++) {
if (moveState[i] == -1) continue; // No active profile
decelerationRequired = GetDecRequired(finalPos[i]-curPos[i], afterBrakeDist[i], curSpeed[i], targetEndSpeed[i] );
if (decelerationRequired>=ABS(curAcc[i]) ) {
// Time to decelerate
#ifndef xxx
if (moveState[i]==0) {
moveState[i] = 1;
targetSpeed[i] = targetEndSpeed[i];
if (targetSpeed[i] == 0) {
// when targetSpeed[i] == 0, the robot might not
// be able to reach the final position due to
// truncation error here and there.
// solution is to temporarily set the speed slightly
// more than zero.
targetSpeed[i]=bDistDirFlag[i]?curAcc[i]:-curAcc[i];
}
}
#else
moveState[i] = 1;
targetSpeed[i] = targetEndSpeed[i];
#endif
// set curAcc[i] to decelerationRequired else the robot
// might not be able to reach targetSpeed[i] when the
// target distance is reached. The result is that the robot
// might need to stop at a high speed which caused it to
// jerk.
// Changing the curAcc[i] here means that we do not know what's
// the curAcc[i] after a speed profile is completed and
// we need to set curAcc[i] again. If you do not want
// curAcc[i] to change, comment off the following line.
// O bizarro é que se eu deixo esta linha após acabar o speed profile com um target speed de zero ele
// mantém uma velocidade bem baixa.
// curAcc[i] = decelerationRequired;
}
if (bDistDirFlag[i]) {
// distance is towards positive direction
if (finalPos[i]<=curPos[i]) {
moveState[i] = -1;
curSpeed[i] = targetSpeed[i] = targetEndSpeed[i];
finalPos[i] = 0;
curPos[i] = 0;
gyroFeedbakEnabled = false;
}
}
else {
if (finalPos[i]>=curPos[i]) {
moveState[i] = -1;
curSpeed[i] = targetSpeed[i] = targetEndSpeed[i];
finalPos[i] = 0;
curPos[i] = 0;
gyroFeedbakEnabled = false;
}
}
}
}
