![](/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@36:474d4795aafa, 2015-09-13 (annotated)
- Committer:
- salco
- Date:
- Sun Sep 13 06:56:09 2015 +0000
- Revision:
- 36:474d4795aafa
- Parent:
- 30:7305ea319fe6
Mise a jours du labyrinthe et augmentation du debogage graphique.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
salco | 36:474d4795aafa | 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 | } |