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@22:a2c875629f2d, 2015-05-08 (annotated)

Committer:
salco
Date:
Fri May 08 19:57:25 2015 +0000
Revision:
22:a2c875629f2d
Parent:
21:5c17b7836733
Child:
27:ea60d12dccdf
turn + recalibrement is donne

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 21:5c17b7836733 71 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 72 m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 73
salco 20:c825e7b07af7 74 tempLeg=m_seq->get_frame(m_arr_G->id());
salco 21:5c17b7836733 75 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 76 m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 77
salco 20:c825e7b07af7 78 tempLeg=m_seq->get_frame(m_arr_mil_D->id());
salco 21:5c17b7836733 79 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 80 m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 81
salco 20:c825e7b07af7 82 tempLeg=m_seq->get_frame(m_arr_mil_G->id());
salco 21:5c17b7836733 83 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 84 m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 85
salco 20:c825e7b07af7 86 tempLeg=m_seq->get_frame(m_avv_mil_D->id());
salco 21:5c17b7836733 87 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 88 m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 89
salco 20:c825e7b07af7 90 tempLeg=m_seq->get_frame(m_avv_mil_G->id());
salco 21:5c17b7836733 91 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 92 m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 93
salco 20:c825e7b07af7 94 tempLeg=m_seq->get_frame(m_avv_D->id());
salco 21:5c17b7836733 95 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 96 m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 97
salco 20:c825e7b07af7 98 tempLeg=m_seq->get_frame(m_avv_G->id());
salco 21:5c17b7836733 99 if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
salco 21:5c17b7836733 100 m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
salco 20:c825e7b07af7 101
salco 20:c825e7b07af7 102 // //...//
salco 0:e4151abdff23 103 if(m_seq->next_frame() == false) {
salco 0:e4151abdff23 104 m_ForceStop=true;
salco 19:0ac80045ba4c 105
salco 21:5c17b7836733 106 debug(DEBUG_EXEC," Flag next impossible \n\r");
salco 19:0ac80045ba4c 107
salco 0:e4151abdff23 108 }
salco 0:e4151abdff23 109 else
salco 21:5c17b7836733 110 debug(DEBUG_EXEC," Flag next possible \n\r");
salco 19:0ac80045ba4c 111
salco 20:c825e7b07af7 112 // break;
salco 20:c825e7b07af7 113 //}
salco 0:e4151abdff23 114 //char buffer[50];
salco 0:e4151abdff23 115 //scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
salco 19:0ac80045ba4c 116 //m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
salco 19:0ac80045ba4c 117
salco 22:a2c875629f2d 118 //debug(DEBUG_MOUVEMENT,"\n\r %s T%d\n\r",m_arr_D->toString(),);
salco 19:0ac80045ba4c 119 sendSequence();
salco 0:e4151abdff23 120 /*
salco 0:e4151abdff23 121 tempLeg=m_seq->get_frame(m_arr_D->id());
salco 0:e4151abdff23 122 m_com->printf("This program is %d .\n", tempLeg[0]);*/
salco 0:e4151abdff23 123 }
salco 19:0ac80045ba4c 124 debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
salco 0:e4151abdff23 125 }
salco 0:e4151abdff23 126 /*void Faculter_motrice::moveLeft (void)
salco 0:e4151abdff23 127 {
salco 0:e4151abdff23 128 m_seq->set_Sequence(9);
salco 0:e4151abdff23 129 }
salco 0:e4151abdff23 130 void Faculter_motrice::moveRight(void)
salco 0:e4151abdff23 131 {
salco 0:e4151abdff23 132
salco 0:e4151abdff23 133 }
salco 0:e4151abdff23 134 void Faculter_motrice::moveUp (void)
salco 0:e4151abdff23 135 {
salco 0:e4151abdff23 136
salco 0:e4151abdff23 137 }
salco 0:e4151abdff23 138 void Faculter_motrice::moveDown (void)
salco 0:e4151abdff23 139 {
salco 0:e4151abdff23 140
salco 0:e4151abdff23 141 }
salco 0:e4151abdff23 142 void Faculter_motrice::moveFront(void)
salco 0:e4151abdff23 143 {
salco 0:e4151abdff23 144
salco 0:e4151abdff23 145 }
salco 0:e4151abdff23 146 void Faculter_motrice::moveBack (void)
salco 0:e4151abdff23 147 {
salco 0:e4151abdff23 148
salco 0:e4151abdff23 149 }
salco 0:e4151abdff23 150 void Faculter_motrice::turnLeft (void)
salco 0:e4151abdff23 151 {
salco 0:e4151abdff23 152
salco 0:e4151abdff23 153 }
salco 0:e4151abdff23 154 void Faculter_motrice::trunRight(void)
salco 0:e4151abdff23 155 {
salco 0:e4151abdff23 156
salco 0:e4151abdff23 157 }*/
salco 0:e4151abdff23 158 void Faculter_motrice::crit_stop(void)
salco 0:e4151abdff23 159 {
salco 0:e4151abdff23 160 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 161 m_ForceStop=true;
salco 0:e4151abdff23 162 m_CriticalStop=true;
salco 19:0ac80045ba4c 163 }
salco 19:0ac80045ba4c 164
salco 19:0ac80045ba4c 165 void Faculter_motrice::sendSequence(void)
salco 19:0ac80045ba4c 166 {
salco 19:0ac80045ba4c 167 debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
salco 22:a2c875629f2d 168 m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
salco 22:a2c875629f2d 169 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
salco 19:0ac80045ba4c 170
salco 22:a2c875629f2d 171 m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
salco 22:a2c875629f2d 172 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
salco 19:0ac80045ba4c 173
salco 22:a2c875629f2d 174 m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
salco 22:a2c875629f2d 175 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
salco 19:0ac80045ba4c 176
salco 22:a2c875629f2d 177 m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
salco 22:a2c875629f2d 178 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
salco 19:0ac80045ba4c 179
salco 22:a2c875629f2d 180 m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
salco 22:a2c875629f2d 181 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
salco 19:0ac80045ba4c 182
salco 22:a2c875629f2d 183 m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
salco 22:a2c875629f2d 184 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
salco 19:0ac80045ba4c 185
salco 22:a2c875629f2d 186 m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
salco 22:a2c875629f2d 187 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
salco 19:0ac80045ba4c 188
salco 22:a2c875629f2d 189 m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
salco 22:a2c875629f2d 190 debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
salco 19:0ac80045ba4c 191
salco 19:0ac80045ba4c 192 debug(DEBUG_SENDSEQ,"\n\r\n\r out sendSeq");
salco 0:e4151abdff23 193 }