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@20:c825e7b07af7, 2015-04-28 (annotated)

Committer:
salco
Date:
Tue Apr 28 01:57:07 2015 +0000
Revision:
20:c825e7b07af7
Parent:
19:0ac80045ba4c
Child:
21:5c17b7836733
walkingcrab is add but we need to compleet the fonction;

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 0:e4151abdff23 14
salco 0:e4151abdff23 15 m_arr_D = new Patte(1,0,1,2,3);
salco 0:e4151abdff23 16 m_arr_mil_D = new Patte(2,4,5,6,7);
salco 0:e4151abdff23 17 m_arr_G = new Patte(3,16,17,18,19);
salco 0:e4151abdff23 18 m_arr_mil_G = new Patte(4,20,21,22,23);
salco 0:e4151abdff23 19 m_avv_D = new Patte(5,12,13,14,15);
salco 0:e4151abdff23 20 m_avv_mil_D = new Patte(6,8,9,10,11);
salco 0:e4151abdff23 21 m_avv_G = new Patte(7,28,29,30,31);
salco 0:e4151abdff23 22 m_avv_mil_G = new Patte(8,24,25,26,27);
salco 0:e4151abdff23 23
salco 0:e4151abdff23 24 m_SequenceID_arr_D =0;
salco 0:e4151abdff23 25 m_SequenceID_arr_mil_D =0;
salco 0:e4151abdff23 26 m_SequenceID_arr_G =0;
salco 0:e4151abdff23 27 m_SequenceID_arr_mil_G =0;
salco 0:e4151abdff23 28 m_SequenceID_avv_D =0;
salco 0:e4151abdff23 29 m_SequenceID_avv_mil_D =0;
salco 0:e4151abdff23 30 m_SequenceID_avv_G =0;
salco 0:e4151abdff23 31 m_SequenceID_avv_mil_G =0;
salco 0:e4151abdff23 32
salco 0:e4151abdff23 33 m_ForceStop = false;
salco 0:e4151abdff23 34 m_CriticalStop = false;
salco 0:e4151abdff23 35 }
salco 0:e4151abdff23 36 Faculter_motrice::~Faculter_motrice()
salco 0:e4151abdff23 37 {
salco 0:e4151abdff23 38 if(m_seq)
salco 0:e4151abdff23 39 delete m_seq;
salco 0:e4151abdff23 40 if (m_arr_D)
salco 0:e4151abdff23 41 delete m_arr_D;
salco 0:e4151abdff23 42 if (m_arr_mil_D)
salco 0:e4151abdff23 43 delete m_arr_mil_D;
salco 0:e4151abdff23 44 if (m_arr_G)
salco 0:e4151abdff23 45 delete m_arr_G;
salco 0:e4151abdff23 46 if (m_arr_mil_G)
salco 0:e4151abdff23 47 delete m_arr_mil_G;
salco 0:e4151abdff23 48
salco 0:e4151abdff23 49 if (m_avv_D)
salco 0:e4151abdff23 50 delete m_avv_D;
salco 0:e4151abdff23 51 if (m_avv_mil_D)
salco 0:e4151abdff23 52 delete m_avv_mil_D;
salco 0:e4151abdff23 53 if (m_avv_G)
salco 0:e4151abdff23 54 delete m_avv_G;
salco 0:e4151abdff23 55 if (m_avv_mil_G)
salco 0:e4151abdff23 56 delete m_avv_mil_G;
salco 0:e4151abdff23 57 return;
salco 0:e4151abdff23 58 }
salco 0:e4151abdff23 59 void Faculter_motrice::exec(void)
salco 0:e4151abdff23 60 {
salco 19:0ac80045ba4c 61 debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott");
salco 19:0ac80045ba4c 62 debug(DEBUG_MOUVEMENT,"\n\r Flag in exec [%i :: %i ] \n\r",m_ForceStop,m_seq->get_Sequence());
salco 0:e4151abdff23 63 if((!m_ForceStop)&&(m_seq->get_Sequence() != 0)) {
salco 0:e4151abdff23 64 unsigned char* tempLeg;
salco 0:e4151abdff23 65
salco 0:e4151abdff23 66
salco 0:e4151abdff23 67
salco 20:c825e7b07af7 68 //switch(m_seq->get_Sequence()) {
salco 20:c825e7b07af7 69 // default:
salco 0:e4151abdff23 70 tempLeg=m_seq->get_frame(m_arr_D->id());
salco 0:e4151abdff23 71 m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 72
salco 20:c825e7b07af7 73 tempLeg=m_seq->get_frame(m_arr_G->id());
salco 20:c825e7b07af7 74 m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 75
salco 20:c825e7b07af7 76 tempLeg=m_seq->get_frame(m_arr_mil_D->id());
salco 20:c825e7b07af7 77 m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 78
salco 20:c825e7b07af7 79 tempLeg=m_seq->get_frame(m_arr_mil_G->id());
salco 20:c825e7b07af7 80 m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 81
salco 20:c825e7b07af7 82 tempLeg=m_seq->get_frame(m_avv_mil_D->id());
salco 20:c825e7b07af7 83 m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 84
salco 20:c825e7b07af7 85 tempLeg=m_seq->get_frame(m_avv_mil_G->id());
salco 20:c825e7b07af7 86 m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 87
salco 20:c825e7b07af7 88 tempLeg=m_seq->get_frame(m_avv_D->id());
salco 20:c825e7b07af7 89 m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 90
salco 20:c825e7b07af7 91 tempLeg=m_seq->get_frame(m_avv_G->id());
salco 20:c825e7b07af7 92 m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 93
salco 20:c825e7b07af7 94 // //...//
salco 0:e4151abdff23 95 if(m_seq->next_frame() == false) {
salco 0:e4151abdff23 96 m_ForceStop=true;
salco 19:0ac80045ba4c 97
salco 19:0ac80045ba4c 98 debug(DEBUG_EXEC," Flag next impossible \n\r",m_ForceStop,m_seq->get_Sequence());
salco 19:0ac80045ba4c 99
salco 0:e4151abdff23 100 }
salco 0:e4151abdff23 101 else
salco 19:0ac80045ba4c 102 debug(DEBUG_EXEC," Flag next possible \n\r",m_ForceStop,m_seq->get_Sequence());
salco 19:0ac80045ba4c 103
salco 20:c825e7b07af7 104 // break;
salco 20:c825e7b07af7 105 //}
salco 0:e4151abdff23 106 //char buffer[50];
salco 0:e4151abdff23 107 //scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 108 //m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
salco 19:0ac80045ba4c 109
salco 20:c825e7b07af7 110 //debug(DEBUG_MOUVEMENT,"\n\r %s T%d\n\r",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 111 sendSequence();
salco 0:e4151abdff23 112 /*
salco 0:e4151abdff23 113 tempLeg=m_seq->get_frame(m_arr_D->id());
salco 0:e4151abdff23 114 m_com->printf("This program is %d .\n", tempLeg[0]);*/
salco 0:e4151abdff23 115 }
salco 19:0ac80045ba4c 116 debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
salco 0:e4151abdff23 117 }
salco 0:e4151abdff23 118 /*void Faculter_motrice::moveLeft (void)
salco 0:e4151abdff23 119 {
salco 0:e4151abdff23 120 m_seq->set_Sequence(9);
salco 0:e4151abdff23 121 }
salco 0:e4151abdff23 122 void Faculter_motrice::moveRight(void)
salco 0:e4151abdff23 123 {
salco 0:e4151abdff23 124
salco 0:e4151abdff23 125 }
salco 0:e4151abdff23 126 void Faculter_motrice::moveUp (void)
salco 0:e4151abdff23 127 {
salco 0:e4151abdff23 128
salco 0:e4151abdff23 129 }
salco 0:e4151abdff23 130 void Faculter_motrice::moveDown (void)
salco 0:e4151abdff23 131 {
salco 0:e4151abdff23 132
salco 0:e4151abdff23 133 }
salco 0:e4151abdff23 134 void Faculter_motrice::moveFront(void)
salco 0:e4151abdff23 135 {
salco 0:e4151abdff23 136
salco 0:e4151abdff23 137 }
salco 0:e4151abdff23 138 void Faculter_motrice::moveBack (void)
salco 0:e4151abdff23 139 {
salco 0:e4151abdff23 140
salco 0:e4151abdff23 141 }
salco 0:e4151abdff23 142 void Faculter_motrice::turnLeft (void)
salco 0:e4151abdff23 143 {
salco 0:e4151abdff23 144
salco 0:e4151abdff23 145 }
salco 0:e4151abdff23 146 void Faculter_motrice::trunRight(void)
salco 0:e4151abdff23 147 {
salco 0:e4151abdff23 148
salco 0:e4151abdff23 149 }*/
salco 0:e4151abdff23 150 void Faculter_motrice::crit_stop(void)
salco 0:e4151abdff23 151 {
salco 0:e4151abdff23 152 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 153 m_ForceStop=true;
salco 0:e4151abdff23 154 m_CriticalStop=true;
salco 19:0ac80045ba4c 155 }
salco 19:0ac80045ba4c 156
salco 19:0ac80045ba4c 157 void Faculter_motrice::sendSequence(void)
salco 19:0ac80045ba4c 158 {
salco 19:0ac80045ba4c 159 debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
salco 19:0ac80045ba4c 160 m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 161 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 162
salco 19:0ac80045ba4c 163 m_com->printf("%s T%d\n\r",m_arr_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 164 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 165
salco 19:0ac80045ba4c 166 m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 167 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 168
salco 19:0ac80045ba4c 169 m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 170 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 171
salco 19:0ac80045ba4c 172 m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 173 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 174
salco 19:0ac80045ba4c 175 m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 176 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 177
salco 19:0ac80045ba4c 178 m_com->printf("%s T%d\n\r",m_avv_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 179 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 180
salco 19:0ac80045ba4c 181 m_com->printf("%s T%d\n\r",m_avv_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 182 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_G->toString(),DELAITESTE);
salco 19:0ac80045ba4c 183
salco 19:0ac80045ba4c 184 debug(DEBUG_SENDSEQ,"\n\r\n\r out sendSeq");
salco 0:e4151abdff23 185 }