Alexandre Salconi-Denis
Programme de contrôle de l'octopode 4DOF, Theraphosa-Salconi.
CtrlBridge
- fonction quelquonque pour communiquer avec les module
- fonction quelquonque pour faire des recherche dans les module dispo
- autre fonction pour jouer avec MemRegistre
Version 1.2.0
- Ajout d'un mode de simulation pour tester le code avec seulement un contrôleur stm32
Diff: source/Motrice/mouvement.cpp
- Revision:
- 27:ea60d12dccdf
- Parent:
- 22:a2c875629f2d
- Child:
- 30:7305ea319fe6
--- a/source/Motrice/mouvement.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/mouvement.cpp Thu May 14 21:40:23 2015 +0000
@@ -11,15 +11,16 @@
Faculter_motrice::Faculter_motrice(Serial* com):m_com(com)
{
m_seq = new homemadeSequence();
+ delaisNextSeq.reset();
m_arr_D = new Patte(1,0,1,2,3);
m_arr_mil_D = new Patte(2,4,5,6,7);
- m_arr_G = new Patte(3,16,17,18,19);
- m_arr_mil_G = new Patte(4,20,21,22,23);
- m_avv_D = new Patte(5,12,13,14,15);
- m_avv_mil_D = new Patte(6,8,9,10,11);
- m_avv_G = new Patte(7,28,29,30,31);
- m_avv_mil_G = new Patte(8,24,25,26,27);
+ m_arr_G = new Patte(5,16,17,18,19);
+ m_arr_mil_G = new Patte(6,20,21,22,23);
+ m_avv_D = new Patte(4,12,13,14,15);
+ m_avv_mil_D = new Patte(3,8,9,10,11);
+ m_avv_G = new Patte(8,28,29,30,31);
+ m_avv_mil_G = new Patte(7,24,25,26,27);
m_SequenceID_arr_D =0;
m_SequenceID_arr_mil_D =0;
@@ -32,6 +33,8 @@
m_ForceStop = false;
m_CriticalStop = false;
+
+
}
Faculter_motrice::~Faculter_motrice()
{
@@ -59,69 +62,81 @@
void Faculter_motrice::exec(void)
{
debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott");
- debug(DEBUG_MOUVEMENT,"\n\r Flag in exec [%i :: %i ] \n\r",m_ForceStop,m_seq->get_Sequence());
+ debug(DEBUG_MOUVEMENT,"\n\r m_ForceStop : %i\n\r Sequence: %i\n\r",m_ForceStop,m_seq->get_Sequence());
if((!m_ForceStop)&&(m_seq->get_Sequence() != 0)) {
- unsigned char* tempLeg;
+ if((delaisNextSeq.read_ms() >= m_seq->get_delaisNeed_Ms()) || (m_seq->get_delaisNeed_Ms() == -1)) {
+
+ delaisNextSeq.stop();
+ debug(DEBUG_MOUVEMENT,"\n\rdelais:%i / mesure:%i",m_seq->get_delaisNeed_Ms(),delaisNextSeq.read_ms());
+ unsigned char* tempLeg;
- //switch(m_seq->get_Sequence()) {
- // default:
- tempLeg=m_seq->get_frame(m_arr_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_mil_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_mil_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_mil_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_mil_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- // //...//
- if(m_seq->next_frame() == false) {
- m_ForceStop=true;
+ //switch(m_seq->get_Sequence()) {
+ // default:
+ tempLeg=m_seq->get_frame(m_arr_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_mil_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_mil_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_mil_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_mil_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
- debug(DEBUG_EXEC," Flag next impossible \n\r");
+ tempLeg=m_seq->get_frame(m_avv_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ // //...//
+
+
+ sendSequence();
- }
- else
- debug(DEBUG_EXEC," Flag next possible \n\r");
+ delaisNextSeq.reset();
+ bool gga =m_seq->next_frame();
+ if(/*m_seq->next_frame()*/gga == false) {
+ m_ForceStop=true;
+
+ debug(DEBUG_EXEC," Flag next impossible \n\r");
+
+ } else {
+ debug(DEBUG_EXEC," Flag next possible \n\r");
+ delaisNextSeq.start();
+ }
+ }
// break;
//}
//char buffer[50];
//scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
//m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
-
+
//debug(DEBUG_MOUVEMENT,"\n\r %s T%d\n\r",m_arr_D->toString(),);
- sendSequence();
+
/*
tempLeg=m_seq->get_frame(m_arr_D->id());
m_com->printf("This program is %d .\n", tempLeg[0]);*/
}
-debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
+ debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
}
/*void Faculter_motrice::moveLeft (void)
{
@@ -155,6 +170,7 @@
{
}*/
+
void Faculter_motrice::crit_stop(void)
{
m_com->printf("#0L #1L #2L #3L #4L #5L #6L #7L #8L #9L #10L #11L #12L #13L #14L #15L #16L #17L #18L #19L #20L #21L #22L #23L #24L #25L #26L #27L #28L #29L #30L #31L #32L\n\r");
@@ -164,30 +180,39 @@
void Faculter_motrice::sendSequence(void)
{
- debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
- m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
-
- debug(DEBUG_SENDSEQ,"\n\r\n\r out sendSeq");
+ debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
+ debug(DEBUG_SENDSEQ,"\n\r tarzan: %i",m_seq->get_msTime(m_arr_mil_D->id()));
+ if(m_arr_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
+ }
+ if(m_arr_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
+ }
+ if(m_arr_mil_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
+ }
+ if(m_arr_mil_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
+ }
+ if(m_avv_mil_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
+ }
+ if(m_avv_mil_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
+ }
+ if(m_avv_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
+ }
+ if(m_avv_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
+ }
+ debug(DEBUG_SENDSEQ,"\n\r out sendSeq\n\r");
}
\ No newline at end of file