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