void setNextPosition(PidMotionDefinition* motionDefinition,
enum InstructionType instructionType,
enum MotionParameterType motionParameterType,
float pNextPosition,
float pa,
float pSpeed) {
if (motionDefinition == NULL) {
writeError(MOTION_DEFINITION_NULL);
return;
}
MotionInstruction* localInst = &(motionDefinition->inst[instructionType]);
localInst->nextPosition = pNextPosition;
localInst->motionParameterType = motionParameterType;
localInst->initialPidType = getPidType(motionParameterType, instructionType);
if (pNextPosition > 0.001f) {
localInst->a = pa;
localInst->speed = pSpeed;
}
// Acceleration and speed becomes negative
else if (pNextPosition < -0.001f) {
localInst->a = -pa;
localInst->speed = -pSpeed;
}
// pNextPosition == 0.0f Don't change the position
else {
if (motionParameterType == MOTION_PARAMETER_TYPE_MAINTAIN_POSITION) {
localInst->a = pa;
localInst->speed = pSpeed;
} else {
// Slavery free
localInst->a = 0.0f;
localInst->speed = 0.0f;
}
}
computeMotionInstruction(localInst);
// When using classic motion, we compute a U value with alpha/theta (2 main parameters), and not with spline (3 values)
motionDefinition->computeU = &simpleMotionUCompute;
}
void setNextPosition(enum InstructionType instructionType,
enum MotionParameterType motionParameterType,
enum PidType pidType,
float pNextPosition,
float pa,
float pSpeed) {
initNextPositionVars(instructionType);
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
MotionInstruction* localInst = &(motionDefinition->inst[instructionType]);
localInst->nextPosition = pNextPosition;
localInst->motionParameterType = motionParameterType;
localInst->pidType = pidType;
if (pNextPosition > 0.001f) {
localInst->a = pa * A_FACTOR;
localInst->speed = pSpeed * SPEED_FACTOR;
}
// Acceleration and speed becomes negative
else if (pNextPosition < -0.001f) {
localInst->a = -pa * A_FACTOR;
localInst->speed = -pSpeed * SPEED_FACTOR;
}
// pNextPosition == 0.0f Don't change the position
else {
if (motionParameterType == MOTION_PARAMETER_TYPE_MAINTAIN_POSITION) {
localInst->a = 1.0f;
localInst->speed = 100.0f;
} else {
// Slavery free
localInst->a = 0.0f;
localInst->speed = 0.0f;
}
}
computeMotionInstruction(localInst);
// When using classic motion, we compute a U value with alpha/theta (2 main parameters), and not with spline (3 values)
pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}
void setNextPosition(int instructionIndex,
unsigned char motionType,
unsigned char pidType,
float pNextPosition,
float pa,
float pSpeed) {
initNextPositionVars(instructionIndex);
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
MotionInstruction* localInst = &(motionDefinition->inst[instructionIndex]);
localInst->nextPosition = pNextPosition;
localInst->motionType = motionType;
localInst->pidType = pidType;
if (pNextPosition > 0.0f) {
localInst->a = pa * A_FACTOR;
localInst->speed = pSpeed * SPEED_FACTOR;
} // Acceleration and speed becomes negative
else if (pNextPosition < 0L) {
localInst->a = -pa * A_FACTOR;
localInst->speed = -pSpeed * SPEED_FACTOR;
} // Don't change the position
else {
if (motionType == MOTION_TYPE_MAINTAIN_POSITION) {
localInst->a = 1.0f;
localInst->speed = 100.0f;
} else {
// Slavery free
localInst->a = 0.0f;
localInst->speed = 0.0f;
}
}
computeMotionInstruction(localInst);
pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}
