Code: void goToPosition(int pos) { // handle moving deposition area to a particular position int potVal = SensorValue(pot); writeDebugStreamLine("pos is %d, potVal is %d", pos, potVal); int distanceToTarget; if (!distanceToTargetCalculated) { distanceToTarget = abs(pos - potVal); distanceToTargetCalculated = true; } else if ((potVal == pos) || ((potVal <= (pos + ERROR_TOLERATION)) && (potVal >= (pos - ERROR_TOLERATION)))) { motor[motorSort] = 0; currentStatus = DEPOSITING; } else { int currentDistanceToTarget; int direction; currentDistanceToTarget = abs(pos - potVal); if (potVal < pos) { direction = 1; } else if (potVal > pos) { direction = -1; } float motorPower = 50 * (currentDistanceToTarget / distanceToTarget); writeDebugStreamLine("initial motorPower is %f", motorPower); if (motorPower < MINUMUM_MOTOR_POWER) { motorPower = MINUMUM_MOTOR_POWER * direction; } else { motorPower *= direction; } writeDebugStreamLine("motorPower is %f, distanceToTarget is %d, currentDistanceToTarget is %d, direction is %d", motorPower, distanceToTarget, currentDistanceToTarget, direction); motor[motorSort] = (int) motorPower; } }
|