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@30:7305ea319fe6, 2015-08-19 (annotated)

Committer:
salco
Date:
Wed Aug 19 18:27:18 2015 +0000
Revision:
30:7305ea319fe6
Parent:
27:ea60d12dccdf
Child:
36:474d4795aafa
Ajout d'un aspect grafique pour simplifier l'analise.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
salco 0:e4151abdff23 1 /*
salco 0:e4151abdff23 2 * mouvement.cpp
salco 0:e4151abdff23 3 *
salco 0:e4151abdff23 4 * Created on: Mar 02, 2015
salco 0:e4151abdff23 5 * Author: salco
salco 0:e4151abdff23 6 */
salco 0:e4151abdff23 7 #include "mouvement.h"
salco 0:e4151abdff23 8
salco 2:4c5b0f6d0a6b 9 Faculter_motrice *ptrFaculterMotrice;
salco 2:4c5b0f6d0a6b 10
salco 0:e4151abdff23 11 Faculter_motrice::Faculter_motrice(Serial* com):m_com(com)
salco 0:e4151abdff23 12 {
salco 0:e4151abdff23 13 m_seq = new homemadeSequence();
salco 27:ea60d12dccdf 14 delaisNextSeq.reset();
salco 0:e4151abdff23 15
salco 0:e4151abdff23 16 m_arr_D = new Patte(1,0,1,2,3);
salco 0:e4151abdff23 17 m_arr_mil_D = new Patte(2,4,5,6,7);
salco 27:ea60d12dccdf 18 m_arr_G = new Patte(5,16,17,18,19);
salco 27:ea60d12dccdf 19 m_arr_mil_G = new Patte(6,20,21,22,23);
salco 27:ea60d12dccdf 20 m_avv_D = new Patte(4,12,13,14,15);
salco 27:ea60d12dccdf 21 m_avv_mil_D = new Patte(3,8,9,10,11);
salco 27:ea60d12dccdf 22 m_avv_G = new Patte(8,28,29,30,31);
salco 27:ea60d12dccdf 23 m_avv_mil_G = new Patte(7,24,25,26,27);
salco 0:e4151abdff23 24
salco 0:e4151abdff23 25 m_SequenceID_arr_D =0;
salco 0:e4151abdff23 26 m_SequenceID_arr_mil_D =0;
salco 0:e4151abdff23 27 m_SequenceID_arr_G =0;
salco 0:e4151abdff23 28 m_SequenceID_arr_mil_G =0;
salco 0:e4151abdff23 29 m_SequenceID_avv_D =0;
salco 0:e4151abdff23 30 m_SequenceID_avv_mil_D =0;
salco 0:e4151abdff23 31 m_SequenceID_avv_G =0;
salco 0:e4151abdff23 32 m_SequenceID_avv_mil_G =0;
salco 0:e4151abdff23 33
salco 0:e4151abdff23 34 m_ForceStop = false;
salco 0:e4151abdff23 35 m_CriticalStop = false;
salco 27:ea60d12dccdf 36
salco 27:ea60d12dccdf 37
salco 0:e4151abdff23 38 }
salco 0:e4151abdff23 39 Faculter_motrice::~Faculter_motrice()
salco 0:e4151abdff23 40 {
salco 0:e4151abdff23 41 if(m_seq)
salco 0:e4151abdff23 42 delete m_seq;
salco 0:e4151abdff23 43 if (m_arr_D)
salco 0:e4151abdff23 44 delete m_arr_D;
salco 0:e4151abdff23 45 if (m_arr_mil_D)
salco 0:e4151abdff23 46 delete m_arr_mil_D;
salco 0:e4151abdff23 47 if (m_arr_G)
salco 0:e4151abdff23 48 delete m_arr_G;
salco 0:e4151abdff23 49 if (m_arr_mil_G)
salco 0:e4151abdff23 50 delete m_arr_mil_G;
salco 0:e4151abdff23 51
salco 0:e4151abdff23 52 if (m_avv_D)
salco 0:e4151abdff23 53 delete m_avv_D;
salco 0:e4151abdff23 54 if (m_avv_mil_D)
salco 0:e4151abdff23 55 delete m_avv_mil_D;
salco 0:e4151abdff23 56 if (m_avv_G)
salco 0:e4151abdff23 57 delete m_avv_G;
salco 0:e4151abdff23 58 if (m_avv_mil_G)
salco 0:e4151abdff23 59 delete m_avv_mil_G;
salco 0:e4151abdff23 60 return;
salco 0:e4151abdff23 61 }
salco 0:e4151abdff23 62 void Faculter_motrice::exec(void)
salco 0:e4151abdff23 63 {
salco 19:0ac80045ba4c 64 debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott");
salco 27:ea60d12dccdf 65 debug(DEBUG_MOUVEMENT,"\n\r m_ForceStop : %i\n\r Sequence: %i\n\r",m_ForceStop,m_seq->get_Sequence());
salco 0:e4151abdff23 66 if((!m_ForceStop)&&(m_seq->get_Sequence() != 0)) {
salco 27:ea60d12dccdf 67 if((delaisNextSeq.read_ms() >= m_seq->get_delaisNeed_Ms()) || (m_seq->get_delaisNeed_Ms() == -1)) {
salco 27:ea60d12dccdf 68
salco 27:ea60d12dccdf 69 delaisNextSeq.stop();
salco 27:ea60d12dccdf 70 debug(DEBUG_MOUVEMENT,"\n\rdelais:%i / mesure:%i",m_seq->get_delaisNeed_Ms(),delaisNextSeq.read_ms());
salco 27:ea60d12dccdf 71 unsigned char* tempLeg;
salco 0:e4151abdff23 72
salco 0:e4151abdff23 73
salco 0:e4151abdff23 74
salco 27:ea60d12dccdf 75 //switch(m_seq->get_Sequence()) {
salco 27:ea60d12dccdf 76 // default:
salco 27:ea60d12dccdf 77 tempLeg=m_seq->get_frame(m_arr_D->id());
salco 27:ea60d12dccdf 78 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 79 m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 80
salco 27:ea60d12dccdf 81 tempLeg=m_seq->get_frame(m_arr_G->id());
salco 27:ea60d12dccdf 82 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 83 m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 84
salco 27:ea60d12dccdf 85 tempLeg=m_seq->get_frame(m_arr_mil_D->id());
salco 27:ea60d12dccdf 86 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 87 m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 88
salco 27:ea60d12dccdf 89 tempLeg=m_seq->get_frame(m_arr_mil_G->id());
salco 27:ea60d12dccdf 90 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 91 m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 92
salco 27:ea60d12dccdf 93 tempLeg=m_seq->get_frame(m_avv_mil_D->id());
salco 27:ea60d12dccdf 94 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 95 m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 96
salco 27:ea60d12dccdf 97 tempLeg=m_seq->get_frame(m_avv_mil_G->id());
salco 27:ea60d12dccdf 98 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 99 m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 19:0ac80045ba4c 100
salco 27:ea60d12dccdf 101 tempLeg=m_seq->get_frame(m_avv_D->id());
salco 27:ea60d12dccdf 102 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 103 m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 104
salco 27:ea60d12dccdf 105 tempLeg=m_seq->get_frame(m_avv_G->id());
salco 27:ea60d12dccdf 106 //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 27:ea60d12dccdf 107 m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 27:ea60d12dccdf 108
salco 27:ea60d12dccdf 109 // //...//
salco 27:ea60d12dccdf 110
salco 27:ea60d12dccdf 111
salco 27:ea60d12dccdf 112 sendSequence();
salco 19:0ac80045ba4c 113
salco 27:ea60d12dccdf 114 delaisNextSeq.reset();
salco 27:ea60d12dccdf 115 bool gga =m_seq->next_frame();
salco 27:ea60d12dccdf 116 if(/*m_seq->next_frame()*/gga == false) {
salco 27:ea60d12dccdf 117 m_ForceStop=true;
salco 27:ea60d12dccdf 118
salco 27:ea60d12dccdf 119 debug(DEBUG_EXEC," Flag next impossible \n\r");
salco 27:ea60d12dccdf 120
salco 27:ea60d12dccdf 121 } else {
salco 27:ea60d12dccdf 122 debug(DEBUG_EXEC," Flag next possible \n\r");
salco 27:ea60d12dccdf 123 delaisNextSeq.start();
salco 27:ea60d12dccdf 124 }
salco 27:ea60d12dccdf 125 }
salco 19:0ac80045ba4c 126
salco 20:c825e7b07af7 127 // break;
salco 20:c825e7b07af7 128 //}
salco 0:e4151abdff23 129 //char buffer[50];
salco 0:e4151abdff23 130 //scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 131 //m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
salco 27:ea60d12dccdf 132
salco 22:a2c875629f2d 133 //debug(DEBUG_MOUVEMENT,"\n\r %s T%d\n\r",m_arr_D->toString(),);
salco 27:ea60d12dccdf 134
salco 0:e4151abdff23 135 /*
salco 0:e4151abdff23 136 tempLeg=m_seq->get_frame(m_arr_D->id());
salco 0:e4151abdff23 137 m_com->printf("This program is %d .\n", tempLeg[0]);*/
salco 0:e4151abdff23 138 }
salco 27:ea60d12dccdf 139 debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
salco 0:e4151abdff23 140 }
salco 0:e4151abdff23 141 /*void Faculter_motrice::moveLeft (void)
salco 0:e4151abdff23 142 {
salco 0:e4151abdff23 143 m_seq->set_Sequence(9);
salco 0:e4151abdff23 144 }
salco 0:e4151abdff23 145 void Faculter_motrice::moveRight(void)
salco 0:e4151abdff23 146 {
salco 0:e4151abdff23 147
salco 0:e4151abdff23 148 }
salco 0:e4151abdff23 149 void Faculter_motrice::moveUp (void)
salco 0:e4151abdff23 150 {
salco 0:e4151abdff23 151
salco 0:e4151abdff23 152 }
salco 0:e4151abdff23 153 void Faculter_motrice::moveDown (void)
salco 0:e4151abdff23 154 {
salco 0:e4151abdff23 155
salco 0:e4151abdff23 156 }
salco 0:e4151abdff23 157 void Faculter_motrice::moveFront(void)
salco 0:e4151abdff23 158 {
salco 0:e4151abdff23 159
salco 0:e4151abdff23 160 }
salco 0:e4151abdff23 161 void Faculter_motrice::moveBack (void)
salco 0:e4151abdff23 162 {
salco 0:e4151abdff23 163
salco 0:e4151abdff23 164 }
salco 0:e4151abdff23 165 void Faculter_motrice::turnLeft (void)
salco 0:e4151abdff23 166 {
salco 0:e4151abdff23 167
salco 0:e4151abdff23 168 }
salco 0:e4151abdff23 169 void Faculter_motrice::trunRight(void)
salco 0:e4151abdff23 170 {
salco 0:e4151abdff23 171
salco 0:e4151abdff23 172 }*/
salco 27:ea60d12dccdf 173
salco 0:e4151abdff23 174 void Faculter_motrice::crit_stop(void)
salco 0:e4151abdff23 175 {
salco 0:e4151abdff23 176 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");
salco 0:e4151abdff23 177 m_ForceStop=true;
salco 0:e4151abdff23 178 m_CriticalStop=true;
salco 19:0ac80045ba4c 179 }
salco 19:0ac80045ba4c 180
salco 19:0ac80045ba4c 181 void Faculter_motrice::sendSequence(void)
salco 30:7305ea319fe6 182 {
salco 30:7305ea319fe6 183
salco 27:ea60d12dccdf 184 debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
salco 27:ea60d12dccdf 185 debug(DEBUG_SENDSEQ,"\n\r tarzan: %i",m_seq->get_msTime(m_arr_mil_D->id()));
salco 27:ea60d12dccdf 186 if(m_arr_D->haveChange()) {
salco 30:7305ea319fe6 187 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 188 m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
salco 30:7305ea319fe6 189 #endif
salco 27:ea60d12dccdf 190 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
salco 27:ea60d12dccdf 191 }
salco 27:ea60d12dccdf 192 if(m_arr_G->haveChange()) {
salco 30:7305ea319fe6 193 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 194 m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
salco 30:7305ea319fe6 195 #endif
salco 27:ea60d12dccdf 196 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
salco 27:ea60d12dccdf 197 }
salco 27:ea60d12dccdf 198 if(m_arr_mil_D->haveChange()) {
salco 30:7305ea319fe6 199 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 200 m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
salco 30:7305ea319fe6 201 #endif
salco 27:ea60d12dccdf 202 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
salco 27:ea60d12dccdf 203 }
salco 27:ea60d12dccdf 204 if(m_arr_mil_G->haveChange()) {
salco 30:7305ea319fe6 205 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 206 m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
salco 30:7305ea319fe6 207 #endif
salco 27:ea60d12dccdf 208 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
salco 27:ea60d12dccdf 209 }
salco 27:ea60d12dccdf 210 if(m_avv_mil_D->haveChange()) {
salco 30:7305ea319fe6 211 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 212 m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
salco 30:7305ea319fe6 213 #endif
salco 27:ea60d12dccdf 214 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
salco 27:ea60d12dccdf 215 }
salco 27:ea60d12dccdf 216 if(m_avv_mil_G->haveChange()) {
salco 30:7305ea319fe6 217 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 218 m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
salco 30:7305ea319fe6 219 #endif
salco 27:ea60d12dccdf 220 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
salco 27:ea60d12dccdf 221 }
salco 27:ea60d12dccdf 222 if(m_avv_D->haveChange()) {
salco 30:7305ea319fe6 223 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 224 m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
salco 30:7305ea319fe6 225 #endif
salco 27:ea60d12dccdf 226 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
salco 27:ea60d12dccdf 227 }
salco 27:ea60d12dccdf 228 if(m_avv_G->haveChange()) {
salco 30:7305ea319fe6 229 #ifndef DEBUG_MOUVEMENT_COMM_OFF
salco 27:ea60d12dccdf 230 m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
salco 30:7305ea319fe6 231 #endif
salco 27:ea60d12dccdf 232 debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
salco 27:ea60d12dccdf 233 }
salco 27:ea60d12dccdf 234 debug(DEBUG_SENDSEQ,"\n\r out sendSeq\n\r");
salco 0:e4151abdff23 235 }