//========================================================================================================================
uint8_t RcSeq_LaunchSequence(const SequenceSt_t *Table)
{
uint8_t Idx, Ret = 0;
for(Idx = 0; Idx < SEQUENCE_MAX_NB; Idx++)
{
if(CmdSequence[Idx].TableOrShortAction == (void*)Table)
{
Ret = ExecuteSequence(CmdSequence[Idx].CmdIdx, CmdSequence[Idx].Pos);
break;
}
}
return(Ret);
}
//========================================================================================================================
uint8_t RcSeq_LaunchShortAction(void(*ShortAction)(void))
{
uint8_t Idx, Ret=0;
for(Idx=0;Idx<SEQUENCE_MAX_NB;Idx++)
{
if(CmdSequence[Idx].TableOrShortAction==(void*)ShortAction)
{
ExecuteSequence(CmdSequence[Idx].CmdIdx,CmdSequence[Idx].Pos);
Ret=1;
break;
}
}
return(Ret);
}
//========================================================================================================================
uint8_t RcSeq_LaunchSequence(SequenceSt_t *Table)
{
uint8_t Idx, Ret=0;
for(Idx=0;Idx<SEQUENCE_MAX_NB;Idx++)
{
if(CmdSequence[Idx] AsMember TableOrShortAction==(void*)Table)
{
ExecuteSequence(CmdSequence[Idx] AsMember CmdIdx,CmdSequence[Idx] AsMember Pos);
Ret=1;
break;
}
}
return(Ret);
}
//========================================================================================================================
void RcSeq_Refresh(void)
{
static uint32_t NowMs=millis();
static uint32_t StartChronoInterPulseMs=millis();
SequenceSt_t *SequenceTable;
void (*ShortAction)(void);
int8_t ShortActionCnt,CmdPos;
uint8_t ChIdx, ServoIdx;
uint32_t RcPulseWidthUs, MotionDurationMs, DebutSeqMs, FinSeqMs, Pos;
uint16_t StartInDegrees, EndInDegrees;
/* Asynchronous RC Command acquisition */
for(ChIdx=0;ChIdx<CmdSignalNb;ChIdx++)
{
if(!RcChannel[ChIdx].Pulse.available()) continue; /* Channel not used or no pulse received */
RcPulseWidthUs=RcChannel[ChIdx].Pulse.width_us();
CmdPos=GetPos(ChIdx,RcPulseWidthUs);
if(CmdPos>=0)
{
if(RcChannel[ChIdx].Pos.Idx!=CmdPos)
{
if((millis()-RcChannel[ChIdx].Pos.StartChronoMs)>=INTER_CMD_DURATION_MS) /* Check the last command was received for at least 1 second */
{
RcChannel[ChIdx].Pos.Idx=CmdPos;
RcChannel[ChIdx].Pos.StartChronoMs=millis();
}
}
else
{
if((millis()-RcChannel[ChIdx].Pos.StartChronoMs)>=((RcChannel[ChIdx].Type==RC_CMD_STICK)?STICK_PULSE_CHECK_MS:KBD_PULSE_CHECK_MS)) /* Check the Pulse is valid at least for 100 ms or 50 ms */
{
ExecuteSequence(ChIdx,CmdPos);
RcChannel[ChIdx].Pos.Idx=NO_POS;
}
}
}
else
{
RcChannel[ChIdx].Pos.Idx=NO_POS;
}
}
NowMs=millis();
if((NowMs - StartChronoInterPulseMs) >= 20L)
{
/* We arrive here every 20 ms */
/* Asynchronous Servo Sequence management */
for(int8_t Idx=0;Idx<SeqNb;Idx++)
{
if(!CmdSequence[Idx].InProgress || !CmdSequence[Idx].SequenceLength) continue;
ShortActionCnt=-1;
for(int8_t SeqLine=0;SeqLine<CmdSequence[Idx].SequenceLength;SeqLine++) /* Read all lines of the sequence table: this allows to run several servos simultaneously (not forcibly one after the other) */
{
SequenceTable=(SequenceSt_t *)CmdSequence[Idx].TableOrShortAction;
ServoIdx=(int8_t)PGM_READ_8(SequenceTable[SeqLine].ServoIndex);
if(ServoIdx==255) /* Not a Servo: it's a short Action to perform only if not already done */
{
ShortActionCnt++;
DebutSeqMs = CmdSequence[Idx].StartChronoMs + (int32_t)PGM_READ_32(SequenceTable[SeqLine].StartMotionOffsetMs);
if( (NowMs>=DebutSeqMs) && !TST_BIT(CmdSequence[Idx].ShortActionMap,ShortActionCnt) )
{
ShortAction=(void(*)(void))PGM_READ_32(SequenceTable[SeqLine].ShortAction);
ShortAction();
SET_BIT(CmdSequence[Idx].ShortActionMap,ShortActionCnt); /* Mark short Action as performed */
/* If the last line contains an Action -> End of Sequence */
if(SeqLine==(CmdSequence[Idx].SequenceLength-1))
{
CmdSequence[Idx].InProgress=0;
CmdSequence[Idx].ShortActionMap=0; /* Mark all Short Action as not performed */
}
}
continue;
}
if(Servo[ServoIdx].RefreshNb && SeqLine!=Servo[ServoIdx].SeqLineInProgress)
{
continue;
}
DebutSeqMs = CmdSequence[Idx].StartChronoMs + (int32_t)PGM_READ_32(SequenceTable[SeqLine].StartMotionOffsetMs);
MotionDurationMs = (int32_t)PGM_READ_32(SequenceTable[SeqLine].MotionDurationMs);
FinSeqMs = DebutSeqMs + MotionDurationMs;
if(!Servo[ServoIdx].RefreshNb && Servo[ServoIdx].SeqLineInProgress==NO_SEQ_LINE)
{
if( (NowMs>=DebutSeqMs) && (NowMs<=FinSeqMs) )
{
Servo[ServoIdx].SeqLineInProgress=SeqLine;
StartInDegrees=(uint16_t)PGM_READ_8(SequenceTable[SeqLine].StartInDegrees);
Servo[ServoIdx].RefreshNb=REFRESH_NB(MotionDurationMs);
Servo[ServoIdx].Motor.write(StartInDegrees);
}
}
else
{
/* A sequence line is in progress: update the next position */
if(Servo[ServoIdx].RefreshNb) Servo[ServoIdx].RefreshNb--;
StartInDegrees=(uint16_t)PGM_READ_8(SequenceTable[SeqLine].StartInDegrees);
EndInDegrees=(uint16_t)PGM_READ_8(SequenceTable[SeqLine].EndInDegrees);
Pos=(int32_t)EndInDegrees-((int32_t)Servo[ServoIdx].RefreshNb*STEP_IN_DEGREES_PER_REFRESH((int32_t)StartInDegrees,(int32_t)EndInDegrees,(int32_t)MotionDurationMs)); //Pour le nombre max de refresh, Pos vaut StartInDegrees
Servo[ServoIdx].Motor.write(Pos);
if( !Servo[ServoIdx].RefreshNb )
{
Servo[ServoIdx].SeqLineInProgress=NO_SEQ_LINE;
/* Last servo motion and refresh = 0 -> End of Sequence */
if(SeqLine==(CmdSequence[Idx].SequenceLength-1))
{
CmdSequence[Idx].InProgress=0;
CmdSequence[Idx].ShortActionMap=0; /* Mark all Short Action as not performed */
}
}
}
}
}
SoftRcPulseOut::refresh(1); /* Force Refresh */
StartChronoInterPulseMs=millis();
}
}
//========================================================================================================================
void RcSeq_Refresh(void)
{
static uint32_t NowMs = millis();
static uint32_t StartChronoInterPulseMs = millis();
SequenceSt_t *SequenceTable;
void (*ShortAction)(void);
int8_t ShortActionCnt;
uint8_t ServoPin, ServoIdx;
uint32_t MotionDurationMs, StartOfSeqMs, EndOfSeqMs, PosUs;
uint16_t StartInUs, EndInUs;
#ifdef RC_SEQ_WITH_SOFT_RC_PULSE_IN_SUPPORT
uint8_t ChIdx;
int8_t CmdPos; /* Shall be signed */
uint32_t RcPulseWidthUs;
/* Asynchronous RC Command acquisition */
for(ChIdx = 0; ChIdx < CmdSignalNb; ChIdx++)
{
if(!RcChannel[ChIdx].Pulse.available()) continue; /* Channel not used or no pulse received */
RcPulseWidthUs = RcChannel[ChIdx].Pulse.width_us();
CmdPos = GetPos(ChIdx, RcPulseWidthUs);
// Serial.print("W=");Serial.print(RcPulseWidthUs);Serial.print(" P=");Serial.println((int)CmdPos);
if(CmdPos >= 0)
{
if(RcChannel[ChIdx].Pos.Idx != CmdPos)
{
RcChannel[ChIdx].Pos.Idx = CmdPos;
RcChannel[ChIdx].Pos.StartChronoMs = millis();
}
else
{
if((millis() - RcChannel[ChIdx].Pos.StartChronoMs) >= ((RcChannel[ChIdx].Type == RC_CMD_STICK)?STICK_PULSE_CHECK_MS:KBD_PULSE_CHECK_MS)) /* Check the Pulse is valid at least for 100 ms or 50 ms */
{
ExecuteSequence(ChIdx, CmdPos);
RcChannel[ChIdx].Pos.Idx = NO_POS;
}
}
}
else
{
RcChannel[ChIdx].Pos.Idx = NO_POS;
}
}
#endif
NowMs = millis();
if((NowMs - StartChronoInterPulseMs) >= 20UL)
{
/* We arrive here every 20 ms */
/* Asynchronous Servo Sequence management */
for(int8_t Idx = 0; Idx < SeqNb; Idx++)
{
if(!CmdSequence[Idx].InProgress || !CmdSequence[Idx].SequenceLength) continue;
ShortActionCnt = -1;
for(int8_t SeqLine = 0; SeqLine < CmdSequence[Idx].SequenceLength; SeqLine++) /* Read all lines of the sequence table: this allows to run several servos simultaneously (not forcibly one after the other) */
{
SequenceTable = (SequenceSt_t *)CmdSequence[Idx].TableOrShortAction;
ServoPin = (int8_t)PGM_READ_8(SequenceTable[SeqLine].ServoPin);
#ifdef RC_SEQ_WITH_SOFT_RC_PULSE_OUT_SUPPORT
ServoIdx = SoftRcPulseOut::getIdByPin(ServoPin);
#endif
#ifdef RC_SEQ_WITH_SHORT_ACTION_SUPPORT
if(ServoPin == 255) /* Not a Servo: it's a short Action to perform only if not already done */
{
ShortActionCnt++;
StartOfSeqMs = CmdSequence[Idx].StartChronoMs + (int32_t)PGM_READ_32(SequenceTable[SeqLine].StartMotionOffsetMs);
if( (NowMs >= StartOfSeqMs) && !TST_BIT(CmdSequence[Idx].ShortActionMap, ShortActionCnt) )
{
ShortAction = (void(*)(void))PGM_READ_16(SequenceTable[SeqLine].ShortAction);
ShortAction();
SET_BIT(CmdSequence[Idx].ShortActionMap, ShortActionCnt); /* Mark short Action as performed */
/* If the last line contains an Action: End of Sequence */
if(SeqLine == (CmdSequence[Idx].SequenceLength - 1))
{
CmdSequence[Idx].InProgress = 0;
CmdSequence[Idx].ShortActionMap = 0; /* Mark all Short Action as not performed */
}
}
continue;
}
#endif
#ifdef RC_SEQ_WITH_SOFT_RC_PULSE_OUT_SUPPORT
if(ServoAttrib[ServoIdx].RefreshNb && SeqLine != ServoAttrib[ServoIdx].SeqLineInProgress)
{
continue;
}
StartOfSeqMs = CmdSequence[Idx].StartChronoMs + (int32_t)PGM_READ_32(SequenceTable[SeqLine].StartMotionOffsetMs);
MotionDurationMs = (int32_t)PGM_READ_16(SequenceTable[SeqLine].MotionDurationMs);
EndOfSeqMs = StartOfSeqMs + MotionDurationMs;
#ifdef RC_SEQ_WITH_STATIC_MEM_ALLOC_SUPPORT
Servo = (SoftRcPulseOut *)&ServoTbl[ServoIdx];
#else
Servo = SoftRcPulseOut::softRcPulseOutById(ServoIdx);
#endif
if(!ServoAttrib[ServoIdx].RefreshNb && ServoAttrib[ServoIdx].SeqLineInProgress == NO_SEQ_LINE)
{
if( (NowMs >= StartOfSeqMs) && (NowMs <= EndOfSeqMs) )
{
ServoAttrib[ServoIdx].SeqLineInProgress = SeqLine;
StartInUs = (uint16_t)PGM_READ_16(SequenceTable[SeqLine].StartInUs);
ServoAttrib[ServoIdx].RefreshNb = REFRESH_NB(MotionDurationMs);
Servo->write_us(StartInUs);
}
}
else
{
/* A sequence line is in progress: update the next position */
if(ServoAttrib[ServoIdx].RefreshNb) ServoAttrib[ServoIdx].RefreshNb--;
StartInUs = (uint16_t)PGM_READ_16(SequenceTable[SeqLine].StartInUs);
EndInUs = (uint16_t)PGM_READ_16(SequenceTable[SeqLine].EndInUs);
PosUs = /*(uint16_t)*/((int32_t)EndInUs - ((int32_t)ServoAttrib[ServoIdx].RefreshNb * STEP_IN_US_PER_REFRESH((int32_t)StartInUs, (int32_t)EndInUs, (int32_t)MotionDurationMs))); //For refresh max nb, Pos = StartInUs
Servo->write_us(PosUs);
if( !ServoAttrib[ServoIdx].RefreshNb )
{
ServoAttrib[ServoIdx].SeqLineInProgress = NO_SEQ_LINE;
/* Last servo motion and refresh = 0 -> End of Sequence */
if(SeqLine == (CmdSequence[Idx].SequenceLength - 1))
{
CmdSequence[Idx].InProgress = 0;
CmdSequence[Idx].ShortActionMap = 0; /* Mark all Short Action as not performed */
#ifdef RC_SEQ_CONTROL_SUPPORT
if(CmdSequence[Idx].Control != NULL) CmdSequence[Idx].Control(RC_SEQ_END_OF_SEQ, Idx);
#endif
}
}
}
#endif
}
}
#ifdef RC_SEQ_WITH_SOFT_RC_PULSE_OUT_SUPPORT
SoftRcPulseOut::refresh(1); /* Force Refresh */
#endif
StartChronoInterPulseMs = millis();
}
}
