void RcSeq_DeclareCommandAndSequence(uint8_t CmdIdx,uint8_t Pos, const SequenceSt_t *Table, uint8_t SequenceLength) #endif { uint8_t Idx, ServoPin; int8_t ServoIdx; uint16_t StartInUs; uint32_t StartMinMs[ALLOC_DEPENDENT_SERVO_MAX_NB]; #ifndef RC_SEQ_WITH_STATIC_MEM_ALLOC_SUPPORT if(!CmdSequence) CmdSequence = (CmdSequenceSt_t*)malloc(sizeof(CmdSequenceSt_t)); else CmdSequence = (CmdSequenceSt_t*)realloc(CmdSequence, sizeof(CmdSequenceSt_t) * (SeqNb + 1)); Idx = SeqNb; SeqNb++; #else for(Idx = 0; Idx < SEQUENCE_MAX_NB; Idx++) { if(!CmdSequence[Idx].TableOrShortAction) { #endif CmdSequence[Idx].CmdIdx = CmdIdx; CmdSequence[Idx].Pos = Pos; CmdSequence[Idx].TableOrShortAction = (void*)Table; CmdSequence[Idx].SequenceLength = SequenceLength; #ifdef RC_SEQ_CONTROL_SUPPORT CmdSequence[Idx].Control = Control; #endif #ifdef RC_SEQ_WITH_STATIC_MEM_ALLOC_SUPPORT SeqNb++; break; } } #endif #ifdef RC_SEQ_WITH_SOFT_RC_PULSE_OUT_SUPPORT /* Get initial pulse width for each Servo */ for(Idx = 0; Idx < ALLOC_DEPENDENT_SERVO_MAX_NB; Idx++) { StartMinMs[Idx] = 0xFFFFFFFF; } for(Idx = 0; Idx < SequenceLength; Idx++) { ServoPin = (int8_t)PGM_READ_8(Table[Idx].ServoPin); ServoIdx = SoftRcPulseOut::getIdByPin(ServoPin); if(ServoPin != 255) { if((uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs) <= StartMinMs[ServoIdx]) { StartMinMs[ServoIdx] = (uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs); StartInUs = (uint16_t)PGM_READ_16(Table[Idx].StartInUs); #ifdef RC_SEQ_WITH_STATIC_MEM_ALLOC_SUPPORT Servo = (SoftRcPulseOut *)&ServoTbl[ServoIdx]; #else Servo = SoftRcPulseOut::softRcPulseOutById(ServoIdx); #endif Servo->write_us(StartInUs); } } } #endif }
//======================================================================================================================== void RcSeq_DeclareCommandAndSequence(uint8_t CmdIdx,uint8_t Pos,SequenceSt_t *Table, uint8_t SequenceLength) { uint8_t Idx, ServoIdx; uint16_t StartInDegrees; uint32_t StartMinMs[SERVO_MAX_NB]; #ifdef RC_SEQ_WITH_STATIC_MEM_ALLOC_SUPPORT for(Idx=0;Idx<SEQUENCE_MAX_NB;Idx++) { if(!CmdSequence[Idx].TableOrShortAction) { CmdSequence[Idx].CmdIdx=CmdIdx; CmdSequence[Idx].Pos=Pos; CmdSequence[Idx].TableOrShortAction=(void*)Table; CmdSequence[Idx].SequenceLength=SequenceLength; SeqNb++; break; } } #else LoadSequenceOrShortAction(CmdIdx,Pos,(void*)Table, SequenceLength); #endif /* Get initial pulse width for each Servo */ for(Idx=0;Idx<SERVO_MAX_NB;Idx++) { StartMinMs[Idx]=0xFFFFFFFF; } for(Idx=0;Idx<SequenceLength;Idx++) { ServoIdx=(int8_t)PGM_READ_8(Table[Idx].ServoIndex); if(ServoIdx!=255) { if((uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs)<=StartMinMs[ServoIdx]) { StartMinMs[ServoIdx]=(uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs); StartInDegrees=(uint16_t)PGM_READ_8(Table[Idx].StartInDegrees); Servo[ServoIdx] AsMember Motor.write(StartInDegrees); } } } }
//======================================================================================================================== void RcSeq_DeclareCommandeEtSequence(uint8_t CmdIdx,uint8_t Pos,SequenceSt_t *Table, uint8_t SequenceLength) { uint8_t Idx, ServoIdx; uint16_t StartInDegrees; uint32_t StartMinMs[SERVO_MAX_NB]; for(Idx=0;Idx<SEQUENCE_MAX_NB;Idx++) { if(!CmdSequence[Idx].TableOrShortAction) { CmdSequence[Idx].CmdIdx=CmdIdx; CmdSequence[Idx].Pos=Pos; CmdSequence[Idx].TableOrShortAction=(void*)Table; CmdSequence[Idx].SequenceLength=SequenceLength; SeqNb++; break; } } /* Get initial pulse width for each Servo */ for(Idx=0;Idx<SERVO_MAX_NB;Idx++) { StartMinMs[Idx]=0xFFFFFFFF; } for(Idx=0;Idx<SequenceLength;Idx++) { ServoIdx=(int8_t)PGM_READ_8(Table[Idx].ServoIndex); if(ServoIdx!=255) { if((uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs)<=StartMinMs[ServoIdx]) { StartMinMs[ServoIdx]=(uint32_t)PGM_READ_32(Table[Idx].StartMotionOffsetMs); StartInDegrees=(uint16_t)PGM_READ_8(Table[Idx].StartInDegrees); Servo[ServoIdx].Motor.write(StartInDegrees); } } } }
//======================================================================================================================== 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(); } }