mouvement.cpp

00001 /**
00002  * mouvement.cpp
00003  *
00004  *  Created on: Mar 02, 2015
00005  *      Author: salco
00006  */
00007 #include "mouvement.h"
00008 
00009 Faculter_motrice *ptrFaculterMotrice;
00010 
00011 Faculter_motrice::Faculter_motrice(Serial* com):m_com(com)
00012 {
00013     m_seq = new homemadeSequence();
00014     delaisNextSeq.reset();
00015 
00016     m_arr_D     = new Patte(1,0,1,2,3);
00017     m_arr_mil_D = new Patte(2,4,5,6,7);
00018     m_arr_G     = new Patte(5,16,17,18,19);
00019     m_arr_mil_G = new Patte(6,20,21,22,23);
00020     m_avv_D     = new Patte(4,12,13,14,15);
00021     m_avv_mil_D = new Patte(3,8,9,10,11);
00022     m_avv_G     = new Patte(8,28,29,30,31);
00023     m_avv_mil_G = new Patte(7,24,25,26,27);
00024 
00025     m_SequenceID_arr_D     =0;
00026     m_SequenceID_arr_mil_D =0;
00027     m_SequenceID_arr_G     =0;
00028     m_SequenceID_arr_mil_G =0;
00029     m_SequenceID_avv_D     =0;
00030     m_SequenceID_avv_mil_D =0;
00031     m_SequenceID_avv_G     =0;
00032     m_SequenceID_avv_mil_G =0;
00033 
00034     m_ForceStop    = false;
00035     m_CriticalStop = false;
00036 
00037 
00038 }
00039 Faculter_motrice::~Faculter_motrice()
00040 {
00041     if(m_seq)
00042         delete m_seq;
00043     if (m_arr_D)
00044         delete m_arr_D;
00045     if (m_arr_mil_D)
00046         delete m_arr_mil_D;
00047     if (m_arr_G)
00048         delete m_arr_G;
00049     if (m_arr_mil_G)
00050         delete m_arr_mil_G;
00051 
00052     if (m_avv_D)
00053         delete m_avv_D;
00054     if (m_avv_mil_D)
00055         delete m_avv_mil_D;
00056     if (m_avv_G)
00057         delete m_avv_G;
00058     if (m_avv_mil_G)
00059         delete m_avv_mil_G;
00060     return;
00061 }
00062 void Faculter_motrice::exec(void)
00063 {
00064     debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott");
00065     debug(DEBUG_MOUVEMENT,"\n\r  m_ForceStop : %i\n\r  Sequence: %i\n\r",m_ForceStop,m_seq->get_Sequence());
00066     if((!m_ForceStop)&&(m_seq->get_Sequence() != 0)) {
00067         if((delaisNextSeq.read_ms() >= m_seq->get_delaisNeed_Ms()) || (m_seq->get_delaisNeed_Ms() == -1)) {
00068             
00069             delaisNextSeq.stop();
00070             debug(DEBUG_MOUVEMENT,"\n\rdelais:%i  / mesure:%i",m_seq->get_delaisNeed_Ms(),delaisNextSeq.read_ms());
00071             unsigned char* tempLeg;
00072 
00073 
00074 
00075             //switch(m_seq->get_Sequence()) {
00076             //    default:
00077             tempLeg=m_seq->get_frame(m_arr_D->id());
00078             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00079             m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00080 
00081             tempLeg=m_seq->get_frame(m_arr_G->id());
00082             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00083             m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00084 
00085             tempLeg=m_seq->get_frame(m_arr_mil_D->id());
00086             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00087             m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00088 
00089             tempLeg=m_seq->get_frame(m_arr_mil_G->id());
00090             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00091             m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00092 
00093             tempLeg=m_seq->get_frame(m_avv_mil_D->id());
00094             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00095             m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00096 
00097             tempLeg=m_seq->get_frame(m_avv_mil_G->id());
00098             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00099             m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00100 
00101             tempLeg=m_seq->get_frame(m_avv_D->id());
00102             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00103             m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00104 
00105             tempLeg=m_seq->get_frame(m_avv_G->id());
00106             //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
00107             m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
00108 
00109             //        //...//
00110 
00111 
00112             sendSequence();
00113 
00114             delaisNextSeq.reset();
00115             bool gga =m_seq->next_frame();
00116             if(/*m_seq->next_frame()*/gga == false) {
00117                 m_ForceStop=true;
00118 
00119                 debug(DEBUG_EXEC,"    Flag next impossible \n\r");
00120 
00121             } else {
00122                 debug(DEBUG_EXEC,"    Flag next possible \n\r");
00123                 delaisNextSeq.start();
00124             }
00125         }
00126 
00127         //        break;
00128         //}
00129         //char buffer[50];
00130         //scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
00131         //m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
00132 
00133         //debug(DEBUG_MOUVEMENT,"\n\r   %s T%d\n\r",m_arr_D->toString(),);
00134 
00135         /*
00136                     tempLeg=m_seq->get_frame(m_arr_D->id());
00137         m_com->printf("This program is %d .\n", tempLeg[0]);*/
00138     }
00139     debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
00140 }
00141 /*void Faculter_motrice::moveLeft (void)
00142 {
00143     m_seq->set_Sequence(9);
00144 }
00145 void Faculter_motrice::moveRight(void)
00146 {
00147 
00148 }
00149 void Faculter_motrice::moveUp   (void)
00150 {
00151 
00152 }
00153 void Faculter_motrice::moveDown (void)
00154 {
00155 
00156 }
00157 void Faculter_motrice::moveFront(void)
00158 {
00159 
00160 }
00161 void Faculter_motrice::moveBack (void)
00162 {
00163 
00164 }
00165 void Faculter_motrice::turnLeft (void)
00166 {
00167 
00168 }
00169 void Faculter_motrice::trunRight(void)
00170 {
00171 
00172 }*/
00173 
00174 void Faculter_motrice::crit_stop(void)
00175 {
00176     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");
00177     m_ForceStop=true;
00178     m_CriticalStop=true;
00179 }
00180 
00181 void Faculter_motrice::sendSequence(void)
00182 {   
00183     
00184     debug(DEBUG_SENDSEQ,"\n\r   in sendSeq\n\r");
00185     debug(DEBUG_SENDSEQ,"\n\r tarzan: %i",m_seq->get_msTime(m_arr_mil_D->id()));
00186     if(m_arr_D->haveChange()) {
00187         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00188         m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
00189         #endif
00190         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
00191     }
00192     if(m_arr_G->haveChange()) {
00193         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00194         m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
00195         #endif
00196         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
00197     }
00198     if(m_arr_mil_D->haveChange()) {
00199         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00200         m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
00201         #endif
00202         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
00203     }
00204     if(m_arr_mil_G->haveChange()) {
00205         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00206         m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
00207         #endif
00208         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
00209     }
00210     if(m_avv_mil_D->haveChange()) {
00211         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00212         m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
00213         #endif
00214         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
00215     }
00216     if(m_avv_mil_G->haveChange()) {
00217         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00218         m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
00219         #endif
00220         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
00221     }
00222     if(m_avv_D->haveChange()) {
00223         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00224         m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
00225         #endif
00226         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
00227     }
00228     if(m_avv_G->haveChange()) {
00229         #ifndef DEBUG_MOUVEMENT_COMM_OFF
00230         m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
00231         #endif
00232         debug(DEBUG_SENDSEQ,"\n\r   %s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
00233     }
00234     debug(DEBUG_SENDSEQ,"\n\r   out sendSeq\n\r");
00235 }