Switching between torque and position

Romke,

You should be running ST_PosCtl even when you are in torque mode.  This way the handoff between the controllers will be smoother.  

Look at lab 05f for an example of switching between two controllers.

I've changed the code now that ST_runPosMove and ST_runPosCtl are also running in torque mode. Also i feed the STPOSMOVE with the current position like this:

// Feed the position feedback into SpinTAC Position Move
STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));

When i switch between them like that i still get a torque spike.

When i disable the SpinTAC position controller before i switch from torque to position mode i don't get the torque spike.

// Disable SpinTAC Position Controller
STPOSCTL_setEnable(stObj->posCtlHandle, false);

It gets enabled again in the normal main loop.

Is this the best way to do it?

Thank you that fixed it.

I'm now trying to switch between torque mode and position velocity mode while the motor is moving.

When i'm switching from position velocity mode to torque mode everything works nice and smooth.

But when i switch from torque to position velocity mode i get a short torque spike.

How can i fix this?

What i am doing now is:

In torque mode:

// Feed the position feedback into SpinTAC Position Move
STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));

// Feed the measured velocity into SpinTAC Position Move
STPOSMOVE_setVelocityStart(stObj->posMoveHandle, STPOSCONV_getVelocityFiltered(stObj->posConvHandle));

And then:

void ST_runPosMoveTorque(ST_Handle handle)
{
	ST_Obj *stObj = (ST_Obj *)handle;

	STPOSMOVE_setEnable(stObj->posMoveHandle, false);
	STPOSMOVE_run(stObj->posMoveHandle);
}

Then i calculate the torque and set the SpdRef:

gMotorVars.CalculatedIqRef_A = _IQmpy(gMotorVars.DesiredCur_A,_IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A));

// set the speed reference so that the forced angle rotates in the correct direction for startup
if(_IQabs(gMotorVars.MeasuredVelQEP_krpm) < _IQ24(0.01))
{
	if(gMotorVars.CalculatedIqRef_A < _IQ24(0.0))
	{
		// Set a negative speed reference to FAST to provide direction information
		CTRL_setSpd_ref_krpm(ctrlHandle, _IQ24(-0.01));
	}
	else if(gMotorVars.CalculatedIqRef_A > _IQ24(0.0))
	{
		// Set a positive speed reference to FAST to provide direction information
		CTRL_setSpd_ref_krpm(ctrlHandle, _IQ24(0.01));
	}
}

After that:

// Feed the torque reference in SpinTAC Position Control
STPOSCTL_setTorqueReference(stObj->posCtlHandle, gMotorVars.CalculatedIqRef_A);

After that i changed the following in ST_runPosCtl for the Torque mode:

// provide the updated references to the SpinTAC Position Control
STPOSCTL_setPositionReference_mrev(stObj->posCtlHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));
STPOSCTL_setVelocityReference(stObj->posCtlHandle, STPOSCONV_getVelocityFiltered(stObj->posConvHandle));

After that i set the calculated Torque

CTRL_setIq_ref_pu(ctrlHandle, gMotorVars.CalculatedIqRef_A);

When i change from torque to position velocity mode i force an update of the ST_POSMOVE

void ST_runPosMove(ST_Handle handle)
{
    ST_Obj *stObj = (ST_Obj *)handle;

    if (ForceUpdate)
    {
        ForceUpdate = false;
        
        // Get the configuration for SpinTAC Velocity Profile Generator
        STPOSMOVE_setCurveType(stObj->posMoveHandle, gMotorVars.SpinTAC.PosMoveCurveType);
        STPOSMOVE_setProfileType(stObj->posMoveHandle, ST_POS_MOVE_VEL_TYPE);
        STPOSMOVE_setVelocityEnd(stObj->posMoveHandle, _IQmpy(gMotorVars.DesiredVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
        // Enable the SpinTAC Position Profile Generator
        STPOSMOVE_setEnable(stObj->posMoveHandle, true);
    }
    else
    {
        // Run SpinTAC Position Profile Generator
        // If we are not running a profile, and the PosStep_MRev has been modified
        if(_IQmpy(gMotorVars.DesiredVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STPOSMOVE_getVelocityEnd(stObj->posMoveHandle)) {
            // Get the configuration for SpinTAC Velocity Profile Generator
            STPOSMOVE_setCurveType(stObj->posMoveHandle, gMotorVars.SpinTAC.PosMoveCurveType);
            STPOSMOVE_setProfileType(stObj->posMoveHandle, ST_POS_MOVE_VEL_TYPE);
            STPOSMOVE_setVelocityEnd(stObj->posMoveHandle, _IQmpy(gMotorVars.DesiredVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
            STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
            STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
            STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
            // Enable the SpinTAC Position Profile Generator
            STPOSMOVE_setEnable(stObj->posMoveHandle, true);
        }
    }

    STPOSMOVE_run(stObj->posMoveHandle);
}

I have Desired_Cur_A set to 0.

When in Position Velocity mode i can set DesiredVel_krpm to 3.7 krpm and the motor goes smooth to 3.7 krpm.

When i change to Torque mode the motor goes smooth to 0 krpm because torque is set to 0.

When i change back to Position Velocity mode i get a small torque spike and then the motor goes smooth again to 3.7 krpm.

Also a way to test this is:

When in Position Velocity mode set DesiredVel_krpm to 3.7 krpm and let the motor go to 3.7 krpm.

Change to torque mode and wait for the motor to be stopped.

Set DesiredVel_krpm to 0.0 krpm.

Change back to Position Velocity Mode.

Now the motor makes a small movement.

I don't have this movement when i first set DesiredVel_krpm to 0.0 in Position Velocity mode and then switch between current and Position Velocity mode.

What can i do to get rid of this torque spike?

I've posted a follow up question, could you please answer this?

Romke,

Have you been able to confirm that all of your references at 0 before changing back to position control?

It also might be worth disabling the Position Controller when you are in torque mode.