998 lines
32 KiB
C++
998 lines
32 KiB
C++
/* Native Seat_Controller_demo rocking-horse and cradle runtime.
|
|
*
|
|
* The 0x300/0x0E command selects one of the original Demo modes. Targets,
|
|
* PWM duties, rendezvous distances, phase timeouts and the 0x306 status frame
|
|
* match Cognitive_Agent-feature-dynamic-mode. All four motors share the
|
|
* production motor/CAN implementation; there is no heartbeat lease. */
|
|
|
|
#define SEAT_DEMO_DYNAMIC_STATUS_ID (0x306ul)
|
|
#define SEAT_DEMO_DYNAMIC_STATUS_MS (100ul)
|
|
#define SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS (600000ul)
|
|
#define SEAT_DEMO_DYNAMIC_ENTRY_TIMEOUT_MS (30000ul)
|
|
#define SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL (3ul)
|
|
#define SEAT_DEMO_DYNAMIC_ENTRY_DUTY (90u)
|
|
#define SEAT_DEMO_DYNAMIC_FINAL_DUTY (50u)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_TIMEOUT_MS (15000ul)
|
|
#define SEAT_DEMO_DYNAMIC_CRADLE_TIMEOUT_MS (30000ul)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_RENDEZVOUS (8ul)
|
|
#define SEAT_DEMO_DYNAMIC_CRADLE_RENDEZVOUS (12ul)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT (2ul)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK (0x03u)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_START_DUTY (50u)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS (1000ul)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS (50ul)
|
|
#define SEAT_DEMO_DYNAMIC_HORSE_DUTY_BOOST_PERCENT (5u)
|
|
#define SEAT_DEMO_DYNAMIC_INVALID_AXIS (0xFFu)
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_DYNAMIC_MODE_NONE = 0u,
|
|
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE = 1u,
|
|
SEAT_DEMO_DYNAMIC_MODE_CRADLE = 2u
|
|
} seat_demo_dynamic_mode_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_DYNAMIC_STATE_IDLE = 0u,
|
|
SEAT_DEMO_DYNAMIC_STATE_ENTRY = 1u,
|
|
SEAT_DEMO_DYNAMIC_STATE_RUNNING = 2u,
|
|
SEAT_DEMO_DYNAMIC_STATE_FAULT = 3u
|
|
} seat_demo_dynamic_state_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_DYNAMIC_PHASE_NONE = 0u,
|
|
SEAT_DEMO_DYNAMIC_PHASE_ENTRY = 1u,
|
|
SEAT_DEMO_DYNAMIC_PHASE_HORSE_FORWARD = 2u,
|
|
SEAT_DEMO_DYNAMIC_PHASE_HORSE_BACKWARD = 3u,
|
|
SEAT_DEMO_DYNAMIC_PHASE_CRADLE_FORWARD = 5u,
|
|
SEAT_DEMO_DYNAMIC_PHASE_CRADLE_RETURN = 6u
|
|
} seat_demo_dynamic_phase_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE = 0u,
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP = 1u,
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_CRUISE = 2u,
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN = 3u,
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING = 4u
|
|
} seat_demo_dynamic_horse_motion_t;
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t mode;
|
|
uint8_t state;
|
|
uint8_t phase;
|
|
uint8_t phaseIndex;
|
|
uint8_t rendezvousStage;
|
|
uint8_t faultMask;
|
|
uint8_t pendingPhaseIndex;
|
|
uint8_t horseMotion;
|
|
uint8_t horseDirectionFarMask;
|
|
uint8_t horseReachedMask;
|
|
uint8_t horseBoostAxis[2];
|
|
uint8_t horseCruiseDuty[SEAT_DEMO_MOTOR_COUNT];
|
|
uint8_t horseAppliedDuty[SEAT_DEMO_MOTOR_COUNT];
|
|
uint8_t horseDecelStartDuty[SEAT_DEMO_MOTOR_COUNT];
|
|
uint16_t cycleIndex;
|
|
uint16_t commandSequence;
|
|
uint32_t modeStartedTicks;
|
|
uint32_t phaseStartedTicks;
|
|
uint32_t phaseTimeoutMs;
|
|
uint32_t horseRampStartedTicks;
|
|
uint32_t targetHall[SEAT_DEMO_MOTOR_COUNT];
|
|
uint32_t finalTargetHall[SEAT_DEMO_MOTOR_COUNT];
|
|
} seat_demo_dynamic_context_t;
|
|
|
|
/* M0/M1/M2/M3 = longitudinal, cushion height, backrest, legrest. */
|
|
static const uint16_t seatDemoDynamicEntry[2][SEAT_DEMO_MOTOR_COUNT] =
|
|
{
|
|
{ 320u, 190u, 500u, 430u },
|
|
{ 391u, 140u, 405u, 404u }
|
|
};
|
|
|
|
static const uint16_t seatDemoDynamicTargets[2][2][SEAT_DEMO_MOTOR_COUNT] =
|
|
{
|
|
{
|
|
{ 270u, 250u, 460u, 400u },
|
|
{ 370u, 130u, 540u, 460u }
|
|
},
|
|
{
|
|
{ 246u, 281u, 701u, 494u },
|
|
{ 391u, 140u, 405u, 404u }
|
|
}
|
|
};
|
|
|
|
static const uint8_t seatDemoDynamicCruiseDuty[2][SEAT_DEMO_MOTOR_COUNT] =
|
|
{
|
|
{ 90u, 90u, 90u, 90u },
|
|
{ 50u, 50u, 90u, 50u }
|
|
};
|
|
|
|
static seat_demo_dynamic_context_t seatDemoDynamic;
|
|
static bool seatDemoDynamicStatusPending;
|
|
static bool seatDemoDynamicSavePending;
|
|
static uint32_t seatDemoDynamicNextStatusTicks;
|
|
|
|
static bool SeatDemoDynamic_IsActive(void)
|
|
{
|
|
return (seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY) ||
|
|
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_RUNNING);
|
|
}
|
|
|
|
static uint8_t SeatDemoDynamic_ActiveMask(void)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t mask = 0u;
|
|
uint32_t interruptState;
|
|
|
|
if(false == SeatDemoDynamic_IsActive())
|
|
{
|
|
return 0u;
|
|
}
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
|
(false != motorRuntime[axis].startPending) ||
|
|
(false != motorRuntime[axis].targetActive) ||
|
|
(false != motorRuntime[axis].coastTracking))
|
|
{
|
|
mask |= (uint8_t)(1u << axis);
|
|
}
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return mask;
|
|
}
|
|
|
|
static uint8_t SeatDemoDynamic_FaultMask(void)
|
|
{
|
|
return seatDemoDynamic.faultMask;
|
|
}
|
|
|
|
static void SeatDemoDynamic_MarkFeedback(void)
|
|
{
|
|
seatDemoDynamicStatusPending = true;
|
|
seatDemoSystemStatusPending = true;
|
|
seatDemoMotorStatusPendingMask |= SEAT_DEMO_ACTIVE_MASK;
|
|
seatDemoDiagnosticPendingMask |= SEAT_DEMO_ACTIVE_MASK;
|
|
}
|
|
|
|
static void SeatDemoDynamic_StopMotors(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
}
|
|
}
|
|
|
|
static bool SeatDemoDynamic_HorseAllMotorsSettled(void)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t interruptState;
|
|
bool settled = true;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
|
(false != motorRuntime[axis].startPending) ||
|
|
(false != motorRuntime[axis].targetActive) ||
|
|
(false != motorRuntime[axis].coastTracking))
|
|
{
|
|
settled = false;
|
|
break;
|
|
}
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return settled;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_AllMotorsSettled(void)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t interruptState;
|
|
bool settled = true;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
|
(false != motorRuntime[axis].startPending) ||
|
|
(false != motorRuntime[axis].targetActive) ||
|
|
(false != motorRuntime[axis].coastTracking))
|
|
{
|
|
settled = false;
|
|
break;
|
|
}
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return settled;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_StartTargets(const uint32_t targets[],
|
|
const uint8_t duties[],
|
|
bool coastPastHorseTargets)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t targetValueX100[SEAT_DEMO_MOTOR_COUNT];
|
|
motor_safety_options_t safety = { false, false };
|
|
|
|
if(false != coastPastHorseTargets)
|
|
{
|
|
seatDemoDynamic.horseDirectionFarMask = 0u;
|
|
}
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
motor_position_info_t info;
|
|
uint32_t commandedTargetHall = targets[axis];
|
|
|
|
if((MOTOR_API_SUCCESS != Motor_GetPositionInfo(
|
|
(motor_mode_t)axis, &info)) ||
|
|
(false == info.positionValid) ||
|
|
(0ul == info.maxHallCount) ||
|
|
(0ul == info.maxValueX100) ||
|
|
(targets[axis] > info.maxHallCount))
|
|
{
|
|
return false;
|
|
}
|
|
if(false != coastPastHorseTargets)
|
|
{
|
|
uint32_t current;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
current = motorRuntime[axis].positionHallCount;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
if(targets[axis] > current)
|
|
{
|
|
commandedTargetHall = info.maxHallCount;
|
|
seatDemoDynamic.horseDirectionFarMask |=
|
|
(uint8_t)(1u << axis);
|
|
}
|
|
else if(targets[axis] < current)
|
|
{
|
|
commandedTargetHall = 0ul;
|
|
}
|
|
/* An exact target is left unchanged. The coupled M0/M1
|
|
* arrival check will immediately start the smooth stop. */
|
|
}
|
|
targetValueX100[axis] = (uint32_t)
|
|
(((uint64_t)commandedTargetHall * info.maxValueX100 +
|
|
(info.maxHallCount / 2ul)) / info.maxHallCount);
|
|
}
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if((MOTOR_API_SUCCESS != Motor_SetSafetyOptions(
|
|
(motor_mode_t)axis, &safety)) ||
|
|
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl(
|
|
(motor_mode_t)axis)) ||
|
|
(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, duties[axis])))
|
|
{
|
|
SeatDemoDynamic_StopMotors();
|
|
return false;
|
|
}
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
|
(motor_mode_t)axis, targetValueX100[axis]))
|
|
{
|
|
SeatDemoDynamic_StopMotors();
|
|
return false;
|
|
}
|
|
otaSeatPersistedMask &= ~(1ul << axis);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_TargetsReached(bool *failed)
|
|
{
|
|
uint32_t axis;
|
|
bool reached = true;
|
|
|
|
*failed = false;
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint32_t position;
|
|
uint32_t difference;
|
|
motor_event_t event;
|
|
bool valid;
|
|
bool moving;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
position = motorRuntime[axis].positionHallCount;
|
|
event = motorRuntime[axis].lastEvent;
|
|
valid = motorRuntime[axis].positionValid;
|
|
moving = (motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
|
(false != motorRuntime[axis].startPending) ||
|
|
(false != motorRuntime[axis].targetActive) ||
|
|
(false != motorRuntime[axis].coastTracking);
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
|
|
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
|
(position - seatDemoDynamic.targetHall[axis]) :
|
|
(seatDemoDynamic.targetHall[axis] - position);
|
|
if((false == valid) ||
|
|
(event == MOTOR_EVENT_STALL) ||
|
|
(event == MOTOR_EVENT_TIMEOUT) ||
|
|
(event == MOTOR_EVENT_DRIVER_FAULT) ||
|
|
(((event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
|
(event == MOTOR_EVENT_FAR_ENDPOINT)) &&
|
|
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)))
|
|
{
|
|
seatDemoDynamic.faultMask |= (uint8_t)(1u << axis);
|
|
*failed = true;
|
|
}
|
|
if((false != moving) ||
|
|
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL))
|
|
{
|
|
reached = false;
|
|
}
|
|
}
|
|
return reached;
|
|
}
|
|
|
|
static uint8_t SeatDemoDynamic_ModeIndex(void)
|
|
{
|
|
return (seatDemoDynamic.mode ==
|
|
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ? 0u : 1u;
|
|
}
|
|
|
|
static uint32_t SeatDemoDynamic_PhaseTimeoutMs(void)
|
|
{
|
|
return (seatDemoDynamic.mode ==
|
|
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ?
|
|
SEAT_DEMO_DYNAMIC_HORSE_TIMEOUT_MS :
|
|
SEAT_DEMO_DYNAMIC_CRADLE_TIMEOUT_MS;
|
|
}
|
|
|
|
static uint32_t SeatDemoDynamic_RendezvousPulses(void)
|
|
{
|
|
return (seatDemoDynamic.mode ==
|
|
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ?
|
|
SEAT_DEMO_DYNAMIC_HORSE_RENDEZVOUS :
|
|
SEAT_DEMO_DYNAMIC_CRADLE_RENDEZVOUS;
|
|
}
|
|
|
|
static void SeatDemoDynamic_ResolveCruiseDuties(
|
|
uint8_t modeIndex,
|
|
uint8_t phaseIndex,
|
|
uint8_t duties[])
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
duties[axis] = seatDemoDynamicCruiseDuty[modeIndex][axis];
|
|
}
|
|
if((seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
|
(phaseIndex < 2u) &&
|
|
(seatDemoDynamic.horseBoostAxis[phaseIndex] <
|
|
SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT))
|
|
{
|
|
uint8_t boostAxis = seatDemoDynamic.horseBoostAxis[phaseIndex];
|
|
uint32_t boosted = (uint32_t)
|
|
duties[boostAxis] +
|
|
SEAT_DEMO_DYNAMIC_HORSE_DUTY_BOOST_PERCENT;
|
|
|
|
if(boosted > MOTOR_FIXED_DUTY_MAX_PERCENT)
|
|
{
|
|
boosted = MOTOR_FIXED_DUTY_MAX_PERCENT;
|
|
}
|
|
duties[boostAxis] = (uint8_t)boosted;
|
|
}
|
|
}
|
|
|
|
static bool SeatDemoDynamic_StartEntry(uint32_t now)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
|
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
|
uint8_t modeIndex = SeatDemoDynamic_ModeIndex();
|
|
|
|
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_ENTRY;
|
|
seatDemoDynamic.phase = SEAT_DEMO_DYNAMIC_PHASE_ENTRY;
|
|
seatDemoDynamic.phaseStartedTicks = now;
|
|
seatDemoDynamic.phaseTimeoutMs = SEAT_DEMO_DYNAMIC_ENTRY_TIMEOUT_MS;
|
|
seatDemoDynamic.rendezvousStage = 0u;
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
targets[axis] = seatDemoDynamicEntry[modeIndex][axis];
|
|
duties[axis] = SEAT_DEMO_DYNAMIC_ENTRY_DUTY;
|
|
seatDemoDynamic.targetHall[axis] = targets[axis];
|
|
seatDemoDynamic.finalTargetHall[axis] = targets[axis];
|
|
}
|
|
SeatDemoDynamic_MarkFeedback();
|
|
return SeatDemoDynamic_StartTargets(targets, duties, false);
|
|
}
|
|
|
|
static bool SeatDemoDynamic_StartPhase(uint8_t phaseIndex, uint32_t now)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
|
uint32_t rendezvous = SeatDemoDynamic_RendezvousPulses();
|
|
uint8_t modeIndex = SeatDemoDynamic_ModeIndex();
|
|
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
|
bool rockingHorse =
|
|
(seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE);
|
|
bool firstFormalPhase =
|
|
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY);
|
|
|
|
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_RUNNING;
|
|
seatDemoDynamic.phaseIndex = phaseIndex;
|
|
seatDemoDynamic.phase = (false != rockingHorse) ?
|
|
(uint8_t)(SEAT_DEMO_DYNAMIC_PHASE_HORSE_FORWARD + phaseIndex) :
|
|
(uint8_t)(SEAT_DEMO_DYNAMIC_PHASE_CRADLE_FORWARD + phaseIndex);
|
|
seatDemoDynamic.phaseStartedTicks = now;
|
|
seatDemoDynamic.phaseTimeoutMs = SeatDemoDynamic_PhaseTimeoutMs();
|
|
seatDemoDynamic.rendezvousStage = (false != rockingHorse) ? 1u : 0u;
|
|
seatDemoDynamic.pendingPhaseIndex = phaseIndex;
|
|
seatDemoDynamic.horseReachedMask = 0u;
|
|
if((false != firstFormalPhase) &&
|
|
(false != rockingHorse))
|
|
{
|
|
/* Entry positioning has its own timeout and is deliberately excluded
|
|
* from the requested rocking-cycle runtime. */
|
|
seatDemoDynamic.modeStartedTicks = now;
|
|
}
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint32_t current;
|
|
uint32_t finalTarget =
|
|
seatDemoDynamicTargets[modeIndex][phaseIndex][axis];
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
current = motorRuntime[axis].positionHallCount;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
seatDemoDynamic.finalTargetHall[axis] = finalTarget;
|
|
if(false != rockingHorse)
|
|
{
|
|
/* The motor core receives the physical endpoint as a guard target
|
|
* so crossing this rocking target does not kill PWM before the
|
|
* coordinated one-second ramp-down has completed. */
|
|
targets[axis] = finalTarget;
|
|
}
|
|
else if(finalTarget > (current + rendezvous))
|
|
{
|
|
targets[axis] = finalTarget - rendezvous;
|
|
}
|
|
else if(current > (finalTarget + rendezvous))
|
|
{
|
|
targets[axis] = finalTarget + rendezvous;
|
|
}
|
|
else
|
|
{
|
|
targets[axis] = finalTarget;
|
|
}
|
|
seatDemoDynamic.targetHall[axis] = targets[axis];
|
|
}
|
|
SeatDemoDynamic_ResolveCruiseDuties(modeIndex, phaseIndex, duties);
|
|
if(false != rockingHorse)
|
|
{
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP;
|
|
seatDemoDynamic.horseRampStartedTicks = now;
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
seatDemoDynamic.horseCruiseDuty[axis] = duties[axis];
|
|
seatDemoDynamic.horseAppliedDuty[axis] =
|
|
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
|
seatDemoDynamic.horseDecelStartDuty[axis] =
|
|
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
|
duties[axis] = SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE;
|
|
}
|
|
SeatDemoDynamic_MarkFeedback();
|
|
return SeatDemoDynamic_StartTargets(targets, duties, rockingHorse);
|
|
}
|
|
|
|
static uint8_t SeatDemoDynamic_HorseReachedMask(void)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t reachedMask = 0u;
|
|
|
|
for(axis = 0ul;
|
|
axis < SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT;
|
|
axis++)
|
|
{
|
|
uint32_t position;
|
|
uint32_t difference;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
position = motorRuntime[axis].positionHallCount;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
|
(position - seatDemoDynamic.targetHall[axis]) :
|
|
(seatDemoDynamic.targetHall[axis] - position);
|
|
if(difference <= SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)
|
|
{
|
|
reachedMask |= (uint8_t)(1u << axis);
|
|
}
|
|
else if(((seatDemoDynamic.horseDirectionFarMask &
|
|
(uint8_t)(1u << axis)) != 0u) &&
|
|
(position > seatDemoDynamic.targetHall[axis]))
|
|
{
|
|
reachedMask |= (uint8_t)(1u << axis);
|
|
}
|
|
else if(((seatDemoDynamic.horseDirectionFarMask &
|
|
(uint8_t)(1u << axis)) == 0u) &&
|
|
(position < seatDemoDynamic.targetHall[axis]))
|
|
{
|
|
reachedMask |= (uint8_t)(1u << axis);
|
|
}
|
|
}
|
|
return reachedMask;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_HorseHasFault(void)
|
|
{
|
|
uint32_t axis;
|
|
bool failed = false;
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint32_t position;
|
|
uint32_t difference;
|
|
motor_event_t event;
|
|
bool valid;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
position = motorRuntime[axis].positionHallCount;
|
|
event = motorRuntime[axis].lastEvent;
|
|
valid = motorRuntime[axis].positionValid;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
|
(position - seatDemoDynamic.targetHall[axis]) :
|
|
(seatDemoDynamic.targetHall[axis] - position);
|
|
if((false == valid) ||
|
|
(event == MOTOR_EVENT_STALL) ||
|
|
(event == MOTOR_EVENT_TIMEOUT) ||
|
|
(event == MOTOR_EVENT_DRIVER_FAULT) ||
|
|
(((event == MOTOR_EVENT_TARGET_REACHED) ||
|
|
(event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
|
(event == MOTOR_EVENT_FAR_ENDPOINT)) &&
|
|
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)))
|
|
{
|
|
seatDemoDynamic.faultMask |= (uint8_t)(1u << axis);
|
|
failed = true;
|
|
}
|
|
}
|
|
return failed;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_HorseApplyDuties(const uint8_t duties[])
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(duties[axis] == seatDemoDynamic.horseAppliedDuty[axis])
|
|
{
|
|
continue;
|
|
}
|
|
if(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, duties[axis]))
|
|
{
|
|
return false;
|
|
}
|
|
seatDemoDynamic.horseAppliedDuty[axis] = duties[axis];
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static uint32_t SeatDemoDynamic_HorseRampElapsedMs(uint32_t now)
|
|
{
|
|
uint32_t elapsedMs = MOTOR_TICKS_TO_MS(
|
|
now - seatDemoDynamic.horseRampStartedTicks);
|
|
|
|
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
|
{
|
|
return SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS;
|
|
}
|
|
return (elapsedMs / SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS) *
|
|
SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS;
|
|
}
|
|
|
|
static bool SeatDemoDynamic_HorseRampUp(uint32_t now)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t elapsedMs = SeatDemoDynamic_HorseRampElapsedMs(now);
|
|
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint32_t span = (uint32_t)
|
|
seatDemoDynamic.horseCruiseDuty[axis] -
|
|
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
|
uint32_t duty = SEAT_DEMO_DYNAMIC_HORSE_START_DUTY +
|
|
((span * elapsedMs +
|
|
(SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS / 2ul)) /
|
|
SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS);
|
|
|
|
duties[axis] = (uint8_t)duty;
|
|
}
|
|
if(false == SeatDemoDynamic_HorseApplyDuties(duties))
|
|
{
|
|
return false;
|
|
}
|
|
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
|
{
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_CRUISE;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void SeatDemoDynamic_HorseBeginRampDown(uint32_t now,
|
|
uint8_t reachedMask)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t nextPhase = (uint8_t)(seatDemoDynamic.phaseIndex + 1u);
|
|
|
|
if(nextPhase >= 2u)
|
|
{
|
|
nextPhase = 0u;
|
|
seatDemoDynamic.cycleIndex++;
|
|
}
|
|
seatDemoDynamic.pendingPhaseIndex = nextPhase;
|
|
seatDemoDynamic.horseReachedMask = reachedMask;
|
|
seatDemoDynamic.horseRampStartedTicks = now;
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN;
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
seatDemoDynamic.horseDecelStartDuty[axis] =
|
|
seatDemoDynamic.horseAppliedDuty[axis];
|
|
}
|
|
SeatDemoDynamic_MarkFeedback();
|
|
}
|
|
|
|
static bool SeatDemoDynamic_HorseRampDown(uint32_t now)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t elapsedMs = SeatDemoDynamic_HorseRampElapsedMs(now);
|
|
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
|
|
|
seatDemoDynamic.horseReachedMask |=
|
|
SeatDemoDynamic_HorseReachedMask();
|
|
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
|
{
|
|
uint8_t missingMask = (uint8_t)
|
|
(SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK &
|
|
(uint8_t)(~seatDemoDynamic.horseReachedMask));
|
|
|
|
if((missingMask != 0u) &&
|
|
(seatDemoDynamic.horseBoostAxis[seatDemoDynamic.phaseIndex] ==
|
|
SEAT_DEMO_DYNAMIC_INVALID_AXIS))
|
|
{
|
|
seatDemoDynamic.horseBoostAxis[seatDemoDynamic.phaseIndex] =
|
|
((missingMask & 0x01u) != 0u) ? 0u : 1u;
|
|
}
|
|
/* Motor_SetDutyPercent deliberately rejects zero. Once the nonlinear
|
|
* duty curve reaches the one-second boundary, stopping the timer is
|
|
* the exact zero-percent step for all four bridges. */
|
|
SeatDemoDynamic_StopMotors();
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING;
|
|
SeatDemoDynamic_MarkFeedback();
|
|
return true;
|
|
}
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint64_t durationSquared =
|
|
((uint64_t)SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS) *
|
|
SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS;
|
|
uint64_t elapsedSquared = ((uint64_t)elapsedMs) * elapsedMs;
|
|
uint64_t remaining = durationSquared - elapsedSquared;
|
|
uint32_t duty = (uint32_t)
|
|
((((uint64_t)seatDemoDynamic.horseDecelStartDuty[axis]) *
|
|
remaining + durationSquared - 1ull) / durationSquared);
|
|
|
|
if(duty < MOTOR_FIXED_DUTY_MIN_PERCENT)
|
|
{
|
|
duty = MOTOR_FIXED_DUTY_MIN_PERCENT;
|
|
}
|
|
duties[axis] = (uint8_t)duty;
|
|
}
|
|
return SeatDemoDynamic_HorseApplyDuties(duties);
|
|
}
|
|
|
|
static bool SeatDemoDynamic_StartFinalApproach(void)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
|
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
|
|
|
seatDemoDynamic.rendezvousStage = 1u;
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
targets[axis] = seatDemoDynamic.finalTargetHall[axis];
|
|
seatDemoDynamic.targetHall[axis] = targets[axis];
|
|
duties[axis] = SEAT_DEMO_DYNAMIC_FINAL_DUTY;
|
|
}
|
|
SeatDemoDynamic_MarkFeedback();
|
|
return SeatDemoDynamic_StartTargets(targets, duties, false);
|
|
}
|
|
|
|
static void SeatDemoDynamic_Stop(uint16_t sequence)
|
|
{
|
|
bool owned = (seatDemoDynamic.mode != SEAT_DEMO_DYNAMIC_MODE_NONE) ||
|
|
(seatDemoDynamic.state != SEAT_DEMO_DYNAMIC_STATE_IDLE);
|
|
|
|
SeatDemoDynamic_StopMotors();
|
|
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
|
seatDemoDynamic.commandSequence = sequence;
|
|
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_IDLE;
|
|
seatDemoDynamicSavePending = true;
|
|
if(false != owned)
|
|
{
|
|
otaSeatJobActive = false;
|
|
otaSeatJobComplete = true;
|
|
otaSeatJobSuccess = true;
|
|
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_STOPPED;
|
|
otaSeatJobAxis = 0x0Fu;
|
|
otaSeatJobError = 0u;
|
|
otaSeatJobProgress = 100u;
|
|
}
|
|
SeatDemoDynamic_MarkFeedback();
|
|
}
|
|
|
|
static void SeatDemoDynamic_Fail(void)
|
|
{
|
|
if(seatDemoDynamic.faultMask == 0u)
|
|
{
|
|
seatDemoDynamic.faultMask = SEAT_DEMO_ACTIVE_MASK;
|
|
}
|
|
SeatDemoDynamic_StopMotors();
|
|
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_FAULT;
|
|
seatDemoDynamicSavePending = true;
|
|
otaSeatJobActive = false;
|
|
otaSeatJobComplete = true;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_STOPPED;
|
|
otaSeatJobAxis = 0x0Fu;
|
|
otaSeatJobError = 1u;
|
|
SeatDemoDynamic_MarkFeedback();
|
|
}
|
|
|
|
static bool SeatDemoDynamic_Start(uint8_t mode, uint16_t sequence)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if((mode != SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
|
(mode != SEAT_DEMO_DYNAMIC_MODE_CRADLE))
|
|
{
|
|
return false;
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(false == SeatDemoCompat_PositionValid(axis))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
|
seatDemoDynamic.mode = mode;
|
|
seatDemoDynamic.horseBoostAxis[0] = SEAT_DEMO_DYNAMIC_INVALID_AXIS;
|
|
seatDemoDynamic.horseBoostAxis[1] = SEAT_DEMO_DYNAMIC_INVALID_AXIS;
|
|
seatDemoDynamic.horseMotion =
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE;
|
|
seatDemoDynamic.commandSequence = sequence;
|
|
seatDemoDynamic.modeStartedTicks = hallTimebaseTicks;
|
|
seatDemoDynamicSavePending = false;
|
|
otaSeatJobActive = true;
|
|
otaSeatJobComplete = false;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_DYNAMIC;
|
|
otaSeatJobAxis = 0x0Fu;
|
|
otaSeatJobError = 0u;
|
|
otaSeatJobProgress = 0u;
|
|
OtaCan_ClearLatchedFaults();
|
|
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
Cy_SysTick_DelayInUs(100ul);
|
|
if(false == SeatDemoDynamic_StartEntry(hallTimebaseTicks))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void SeatDemoDynamic_Service(void)
|
|
{
|
|
uint32_t now = hallTimebaseTicks;
|
|
bool failed;
|
|
bool reached;
|
|
|
|
if(false == SeatDemoDynamic_IsActive())
|
|
{
|
|
if((false != seatDemoDynamicSavePending) &&
|
|
(false != SeatDemoDynamic_AllMotorsSettled()))
|
|
{
|
|
(void)OtaSeat_SavePersistentState();
|
|
seatDemoDynamicSavePending = false;
|
|
}
|
|
return;
|
|
}
|
|
if(false != driverFaultLatched)
|
|
{
|
|
seatDemoDynamic.faultMask = SEAT_DEMO_ACTIVE_MASK;
|
|
SeatDemoDynamic_Fail();
|
|
return;
|
|
}
|
|
if((seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY) &&
|
|
(seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE))
|
|
{
|
|
otaSeatJobProgress = 0u;
|
|
}
|
|
else
|
|
{
|
|
otaSeatJobProgress = (uint8_t)
|
|
((MOTOR_TICKS_TO_MS(now - seatDemoDynamic.modeStartedTicks) *
|
|
100ul) / SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS);
|
|
if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoDynamic.modeStartedTicks +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS)))
|
|
{
|
|
SeatDemoDynamic_Stop((uint16_t)
|
|
(seatDemoDynamic.commandSequence + 1u));
|
|
return;
|
|
}
|
|
}
|
|
|
|
if((seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
|
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_RUNNING))
|
|
{
|
|
uint8_t reachedMask;
|
|
|
|
if(false != SeatDemoDynamic_HorseHasFault())
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
return;
|
|
}
|
|
if(seatDemoDynamic.horseMotion ==
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING)
|
|
{
|
|
if(false != SeatDemoDynamic_HorseAllMotorsSettled())
|
|
{
|
|
if(false == SeatDemoDynamic_StartPhase(
|
|
seatDemoDynamic.pendingPhaseIndex, now))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
}
|
|
else if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoDynamic.phaseStartedTicks +
|
|
MOTOR_MS_TO_TICKS(
|
|
seatDemoDynamic.phaseTimeoutMs)))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
return;
|
|
}
|
|
if(seatDemoDynamic.horseMotion ==
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN)
|
|
{
|
|
if(false == SeatDemoDynamic_HorseRampDown(now))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
return;
|
|
}
|
|
if(seatDemoDynamic.horseMotion ==
|
|
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP)
|
|
{
|
|
if(false == SeatDemoDynamic_HorseRampUp(now))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
return;
|
|
}
|
|
}
|
|
reachedMask = SeatDemoDynamic_HorseReachedMask();
|
|
if((reachedMask & SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK) != 0u)
|
|
{
|
|
SeatDemoDynamic_HorseBeginRampDown(now, reachedMask);
|
|
return;
|
|
}
|
|
if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoDynamic.phaseStartedTicks +
|
|
MOTOR_MS_TO_TICKS(seatDemoDynamic.phaseTimeoutMs)))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
return;
|
|
}
|
|
|
|
reached = SeatDemoDynamic_TargetsReached(&failed);
|
|
if(false != failed)
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
return;
|
|
}
|
|
if(false != reached)
|
|
{
|
|
if(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY)
|
|
{
|
|
if(false == SeatDemoDynamic_StartPhase(0u, now))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
}
|
|
else if(seatDemoDynamic.rendezvousStage == 0u)
|
|
{
|
|
if(false == SeatDemoDynamic_StartFinalApproach())
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
uint8_t nextPhase = (uint8_t)(seatDemoDynamic.phaseIndex + 1u);
|
|
if(nextPhase >= 2u)
|
|
{
|
|
nextPhase = 0u;
|
|
seatDemoDynamic.cycleIndex++;
|
|
}
|
|
if(false == SeatDemoDynamic_StartPhase(nextPhase, now))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoDynamic.phaseStartedTicks +
|
|
MOTOR_MS_TO_TICKS(seatDemoDynamic.phaseTimeoutMs)))
|
|
{
|
|
SeatDemoDynamic_Fail();
|
|
}
|
|
}
|
|
|
|
static bool SeatDemoDynamic_SendStatus(void)
|
|
{
|
|
uint8_t data[8];
|
|
uint32_t elapsedMs = 0ul;
|
|
|
|
if(false != SeatDemoDynamic_IsActive())
|
|
{
|
|
elapsedMs = MOTOR_TICKS_TO_MS(
|
|
hallTimebaseTicks - seatDemoDynamic.phaseStartedTicks);
|
|
if(elapsedMs > 0xFFFFul)
|
|
{
|
|
elapsedMs = 0xFFFFul;
|
|
}
|
|
}
|
|
data[0] = seatDemoDynamic.mode;
|
|
data[1] = seatDemoDynamic.state;
|
|
data[2] = seatDemoDynamic.phase;
|
|
data[3] = (uint8_t)((SeatDemoDynamic_ActiveMask() & 0x0Fu) |
|
|
((seatDemoDynamic.faultMask & 0x0Fu) << 4u));
|
|
SeatDemoCompat_WriteLe16(&data[4], seatDemoDynamic.cycleIndex);
|
|
SeatDemoCompat_WriteLe16(&data[6], (uint16_t)elapsedMs);
|
|
return OtaCan_QueueFrame(SEAT_DEMO_DYNAMIC_STATUS_ID, data);
|
|
}
|
|
|
|
static void SeatDemoDynamic_Init(void)
|
|
{
|
|
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
|
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_IDLE;
|
|
seatDemoDynamicStatusPending = true;
|
|
seatDemoDynamicSavePending = false;
|
|
seatDemoDynamicNextStatusTicks = hallTimebaseTicks;
|
|
}
|