Alexandre Salconi-Denis / Mbed 2 deprecated ProjetOctopode

Programme de contrôle de l'octopode 4DOF, Theraphosa-Salconi.

Dependencies:   debug mbed

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:
2015-08-19
Revision:
30:7305ea319fe6
Parent:
27:ea60d12dccdf
Child:
36:474d4795aafa

File content as of revision 30:7305ea319fe6:

/*
 * 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");
}