![](/media/cache/profiles/4b0986230d9d7aded7f0c24033d221d4.jpg.50x50_q85.jpg)
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
source/Motrice/mouvement.cpp
- Committer:
- salco
- Date:
- 2017-01-31
- Revision:
- 41:0b5c14dda54a
- Parent:
- 36:474d4795aafa
File content as of revision 41:0b5c14dda54a:
/** * mouvement.cpp * * Created on: Mar 02, 2015 * Author: salco */ #include "mouvement.h" Faculter_motrice *ptrFaculterMotrice; 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(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; m_SequenceID_arr_G =0; m_SequenceID_arr_mil_G =0; m_SequenceID_avv_D =0; m_SequenceID_avv_mil_D =0; m_SequenceID_avv_G =0; m_SequenceID_avv_mil_G =0; m_ForceStop = false; m_CriticalStop = false; } Faculter_motrice::~Faculter_motrice() { if(m_seq) delete m_seq; if (m_arr_D) delete m_arr_D; if (m_arr_mil_D) delete m_arr_mil_D; if (m_arr_G) delete m_arr_G; if (m_arr_mil_G) delete m_arr_mil_G; if (m_avv_D) delete m_avv_D; if (m_avv_mil_D) delete m_avv_mil_D; if (m_avv_G) delete m_avv_G; if (m_avv_mil_G) delete m_avv_mil_G; return; } void Faculter_motrice::exec(void) { debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott"); 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)) { 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]); // //...// sendSequence(); 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(),); /* 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"); } /*void Faculter_motrice::moveLeft (void) { m_seq->set_Sequence(9); } void Faculter_motrice::moveRight(void) { } void Faculter_motrice::moveUp (void) { } void Faculter_motrice::moveDown (void) { } void Faculter_motrice::moveFront(void) { } void Faculter_motrice::moveBack (void) { } void Faculter_motrice::turnLeft (void) { } void Faculter_motrice::trunRight(void) { }*/ 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"); m_ForceStop=true; m_CriticalStop=true; } void Faculter_motrice::sendSequence(void) { 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id())); #endif 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()) { #ifndef DEBUG_MOUVEMENT_COMM_OFF m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id())); #endif 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"); }