Alexandre Salconi-Denis
/
ProjetOctopode
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/homemadeSequence.cpp@1:25ea21da4542, 2015-03-11 (annotated)
- Committer:
- salco
- Date:
- Wed Mar 11 21:57:26 2015 +0000
- Revision:
- 1:25ea21da4542
- Parent:
- source/homemadeSequence.cpp@0:e4151abdff23
Changer les doccuments pour des noms plus generique et creation de nouveau sous repertoire.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
salco | 0:e4151abdff23 | 1 | /* |
salco | 0:e4151abdff23 | 2 | * homemadeSequence.cpp |
salco | 0:e4151abdff23 | 3 | * |
salco | 0:e4151abdff23 | 4 | * Created on: Mar 03, 2015 |
salco | 0:e4151abdff23 | 5 | * Author: salco |
salco | 0:e4151abdff23 | 6 | */ |
salco | 0:e4151abdff23 | 7 | #include "homemadeSequence.h" |
salco | 0:e4151abdff23 | 8 | homemadeSequence::homemadeSequence() |
salco | 0:e4151abdff23 | 9 | { |
salco | 0:e4151abdff23 | 10 | m_SequenceChosen=0; |
salco | 0:e4151abdff23 | 11 | m_idFrame=0; |
salco | 0:e4151abdff23 | 12 | } |
salco | 0:e4151abdff23 | 13 | homemadeSequence::~homemadeSequence() |
salco | 0:e4151abdff23 | 14 | { |
salco | 0:e4151abdff23 | 15 | |
salco | 0:e4151abdff23 | 16 | } |
salco | 0:e4151abdff23 | 17 | unsigned char* homemadeSequence::get_frame(/*char idSequence,char idOperation,*/char idLeg) |
salco | 0:e4151abdff23 | 18 | { |
salco | 0:e4151abdff23 | 19 | m_posLeg[0] = 0; |
salco | 0:e4151abdff23 | 20 | m_posLeg[1] = 0; |
salco | 0:e4151abdff23 | 21 | m_posLeg[2] = 0; |
salco | 0:e4151abdff23 | 22 | m_posLeg[3] = 0; |
salco | 0:e4151abdff23 | 23 | |
salco | 0:e4151abdff23 | 24 | switch(m_SequenceChosen) { |
salco | 0:e4151abdff23 | 25 | case 1:// 01 - Calibration(milieu) // |
salco | 0:e4151abdff23 | 26 | if(m_idFrame == 1) {//if(idOperation == 1) { |
salco | 0:e4151abdff23 | 27 | m_posLeg[0] = 150; |
salco | 0:e4151abdff23 | 28 | m_posLeg[1] = 150; |
salco | 0:e4151abdff23 | 29 | m_posLeg[2] = 150; |
salco | 0:e4151abdff23 | 30 | m_posLeg[3] = 150; |
salco | 0:e4151abdff23 | 31 | } |
salco | 0:e4151abdff23 | 32 | break; |
salco | 0:e4151abdff23 | 33 | |
salco | 0:e4151abdff23 | 34 | case 2:// 02 - Debout // |
salco | 0:e4151abdff23 | 35 | seqUpDown(true,m_idFrame,idLeg);//seqUpDown(true,idOperation,idLeg); |
salco | 0:e4151abdff23 | 36 | break; |
salco | 0:e4151abdff23 | 37 | case 3:// 03 - Coucher // |
salco | 0:e4151abdff23 | 38 | seqUpDown(false,m_idFrame,idLeg);//seqUpDown(false,idOperation,idLeg); |
salco | 0:e4151abdff23 | 39 | break; |
salco | 0:e4151abdff23 | 40 | case 4:// 04 - Tourne Gauche // |
salco | 0:e4151abdff23 | 41 | break; |
salco | 0:e4151abdff23 | 42 | case 5:// 05 - Tourne Droite // |
salco | 0:e4151abdff23 | 43 | break; |
salco | 0:e4151abdff23 | 44 | case 6:// 06 - Marche // |
salco | 0:e4151abdff23 | 45 | break; |
salco | 0:e4151abdff23 | 46 | case 7:// 07 - Recule // |
salco | 0:e4151abdff23 | 47 | break; |
salco | 0:e4151abdff23 | 48 | case 8:// 07 - Repositioner // |
salco | 0:e4151abdff23 | 49 | break; |
salco | 0:e4151abdff23 | 50 | case 9:// 09 - Crabe Gauche // |
salco | 0:e4151abdff23 | 51 | break; |
salco | 0:e4151abdff23 | 52 | case 10:// 10 - Crabe Droite // |
salco | 0:e4151abdff23 | 53 | break; |
salco | 0:e4151abdff23 | 54 | } |
salco | 0:e4151abdff23 | 55 | return m_posLeg; |
salco | 0:e4151abdff23 | 56 | } |
salco | 0:e4151abdff23 | 57 | |
salco | 0:e4151abdff23 | 58 | void homemadeSequence::seqUpDown(bool downUP,char idOperation,char idLeg) |
salco | 0:e4151abdff23 | 59 | { |
salco | 0:e4151abdff23 | 60 | if((idOperation<10)&&(idOperation>0)) { |
salco | 0:e4151abdff23 | 61 | switch(idLeg) { |
salco | 0:e4151abdff23 | 62 | case 1: |
salco | 0:e4151abdff23 | 63 | m_posLeg[0] = 140; |
salco | 0:e4151abdff23 | 64 | break; |
salco | 0:e4151abdff23 | 65 | case 2: |
salco | 0:e4151abdff23 | 66 | m_posLeg[0] = 165; |
salco | 0:e4151abdff23 | 67 | break; |
salco | 0:e4151abdff23 | 68 | case 3: |
salco | 0:e4151abdff23 | 69 | m_posLeg[0] = 160; |
salco | 0:e4151abdff23 | 70 | break; |
salco | 0:e4151abdff23 | 71 | case 6: |
salco | 0:e4151abdff23 | 72 | m_posLeg[0] = 133; |
salco | 0:e4151abdff23 | 73 | break; |
salco | 0:e4151abdff23 | 74 | case 7: |
salco | 0:e4151abdff23 | 75 | m_posLeg[0] = 170; |
salco | 0:e4151abdff23 | 76 | break; |
salco | 0:e4151abdff23 | 77 | default: |
salco | 0:e4151abdff23 | 78 | m_posLeg[0] = 150; |
salco | 0:e4151abdff23 | 79 | break; |
salco | 0:e4151abdff23 | 80 | } |
salco | 0:e4151abdff23 | 81 | if(idOperation == 1) { |
salco | 0:e4151abdff23 | 82 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 83 | m_posLeg[1] = 80; |
salco | 0:e4151abdff23 | 84 | m_posLeg[2] = 210; |
salco | 0:e4151abdff23 | 85 | m_posLeg[3] = 160; |
salco | 0:e4151abdff23 | 86 | } else { |
salco | 0:e4151abdff23 | 87 | m_posLeg[1] = 220; |
salco | 0:e4151abdff23 | 88 | m_posLeg[2] = 90; |
salco | 0:e4151abdff23 | 89 | m_posLeg[3] = 114; |
salco | 0:e4151abdff23 | 90 | } |
salco | 0:e4151abdff23 | 91 | |
salco | 0:e4151abdff23 | 92 | } else { |
salco | 0:e4151abdff23 | 93 | if(!downUP) { |
salco | 0:e4151abdff23 | 94 | if(idOperation == 9) { |
salco | 0:e4151abdff23 | 95 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 96 | m_posLeg[1] = 80; |
salco | 0:e4151abdff23 | 97 | m_posLeg[2] = 100; |
salco | 0:e4151abdff23 | 98 | m_posLeg[3] = 150; |
salco | 0:e4151abdff23 | 99 | } else { |
salco | 0:e4151abdff23 | 100 | m_posLeg[1] = 220; |
salco | 0:e4151abdff23 | 101 | m_posLeg[2] = 200; |
salco | 0:e4151abdff23 | 102 | m_posLeg[3] = 150; |
salco | 0:e4151abdff23 | 103 | } |
salco | 0:e4151abdff23 | 104 | } |
salco | 0:e4151abdff23 | 105 | idOperation = 10 - idOperation; |
salco | 0:e4151abdff23 | 106 | } |
salco | 0:e4151abdff23 | 107 | |
salco | 0:e4151abdff23 | 108 | switch(idOperation) { |
salco | 0:e4151abdff23 | 109 | /*case 1: |
salco | 0:e4151abdff23 | 110 | |
salco | 0:e4151abdff23 | 111 | break;*/ |
salco | 0:e4151abdff23 | 112 | case 2: |
salco | 0:e4151abdff23 | 113 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 114 | m_posLeg[1] = 80; |
salco | 0:e4151abdff23 | 115 | m_posLeg[2] = 220; |
salco | 0:e4151abdff23 | 116 | m_posLeg[3] = 170; |
salco | 0:e4151abdff23 | 117 | } else { |
salco | 0:e4151abdff23 | 118 | m_posLeg[1] = 220; |
salco | 0:e4151abdff23 | 119 | m_posLeg[2] = 80; |
salco | 0:e4151abdff23 | 120 | m_posLeg[3] = 130; |
salco | 0:e4151abdff23 | 121 | } |
salco | 0:e4151abdff23 | 122 | break; |
salco | 0:e4151abdff23 | 123 | case 3: |
salco | 0:e4151abdff23 | 124 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 125 | m_posLeg[1] = 90; |
salco | 0:e4151abdff23 | 126 | m_posLeg[2] = 220; |
salco | 0:e4151abdff23 | 127 | m_posLeg[3] = 160; |
salco | 0:e4151abdff23 | 128 | } else { |
salco | 0:e4151abdff23 | 129 | m_posLeg[1] = 200; |
salco | 0:e4151abdff23 | 130 | m_posLeg[2] = 200; |
salco | 0:e4151abdff23 | 131 | m_posLeg[3] = 140; |
salco | 0:e4151abdff23 | 132 | } |
salco | 0:e4151abdff23 | 133 | break; |
salco | 0:e4151abdff23 | 134 | case 4: |
salco | 0:e4151abdff23 | 135 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 136 | m_posLeg[1] = 106; |
salco | 0:e4151abdff23 | 137 | m_posLeg[2] = 220; |
salco | 0:e4151abdff23 | 138 | m_posLeg[3] = 150; |
salco | 0:e4151abdff23 | 139 | } else { |
salco | 0:e4151abdff23 | 140 | m_posLeg[1] = 195; |
salco | 0:e4151abdff23 | 141 | m_posLeg[2] = 80; |
salco | 0:e4151abdff23 | 142 | m_posLeg[3] = 150; |
salco | 0:e4151abdff23 | 143 | } |
salco | 0:e4151abdff23 | 144 | break; |
salco | 0:e4151abdff23 | 145 | case 5: |
salco | 0:e4151abdff23 | 146 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 147 | m_posLeg[1] = 128; |
salco | 0:e4151abdff23 | 148 | m_posLeg[2] = 220; |
salco | 0:e4151abdff23 | 149 | m_posLeg[3] = 128; |
salco | 0:e4151abdff23 | 150 | } else { |
salco | 0:e4151abdff23 | 151 | m_posLeg[1] = 172; |
salco | 0:e4151abdff23 | 152 | m_posLeg[2] = 80; |
salco | 0:e4151abdff23 | 153 | m_posLeg[3] = 173; |
salco | 0:e4151abdff23 | 154 | } |
salco | 0:e4151abdff23 | 155 | break; |
salco | 0:e4151abdff23 | 156 | case 6: |
salco | 0:e4151abdff23 | 157 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 158 | m_posLeg[1] = 144; |
salco | 0:e4151abdff23 | 159 | m_posLeg[2] = 205; |
salco | 0:e4151abdff23 | 160 | m_posLeg[3] = 130; |
salco | 0:e4151abdff23 | 161 | } else { |
salco | 0:e4151abdff23 | 162 | m_posLeg[1] = 156; |
salco | 0:e4151abdff23 | 163 | m_posLeg[2] = 95; |
salco | 0:e4151abdff23 | 164 | m_posLeg[3] = 170; |
salco | 0:e4151abdff23 | 165 | } |
salco | 0:e4151abdff23 | 166 | break; |
salco | 0:e4151abdff23 | 167 | case 7: |
salco | 0:e4151abdff23 | 168 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 169 | m_posLeg[1] = 144; |
salco | 0:e4151abdff23 | 170 | m_posLeg[2] = 211; |
salco | 0:e4151abdff23 | 171 | m_posLeg[3] = 123; |
salco | 0:e4151abdff23 | 172 | } else { |
salco | 0:e4151abdff23 | 173 | m_posLeg[1] = 156; |
salco | 0:e4151abdff23 | 174 | m_posLeg[2] = 89; |
salco | 0:e4151abdff23 | 175 | m_posLeg[3] = 177; |
salco | 0:e4151abdff23 | 176 | } |
salco | 0:e4151abdff23 | 177 | break; |
salco | 0:e4151abdff23 | 178 | case 8: |
salco | 0:e4151abdff23 | 179 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 180 | m_posLeg[1] = 172; |
salco | 0:e4151abdff23 | 181 | m_posLeg[2] = 185; |
salco | 0:e4151abdff23 | 182 | m_posLeg[3] = 124; |
salco | 0:e4151abdff23 | 183 | } else { |
salco | 0:e4151abdff23 | 184 | m_posLeg[1] = 128; |
salco | 0:e4151abdff23 | 185 | m_posLeg[2] = 115; |
salco | 0:e4151abdff23 | 186 | m_posLeg[3] = 176; |
salco | 0:e4151abdff23 | 187 | } |
salco | 0:e4151abdff23 | 188 | break; |
salco | 0:e4151abdff23 | 189 | case 9: |
salco | 0:e4151abdff23 | 190 | if(idLeg<5) { |
salco | 0:e4151abdff23 | 191 | m_posLeg[1] = 194; |
salco | 0:e4151abdff23 | 192 | m_posLeg[2] = 166; |
salco | 0:e4151abdff23 | 193 | m_posLeg[3] = 118; |
salco | 0:e4151abdff23 | 194 | } else { |
salco | 0:e4151abdff23 | 195 | m_posLeg[1] = 106; |
salco | 0:e4151abdff23 | 196 | m_posLeg[2] = 134; |
salco | 0:e4151abdff23 | 197 | m_posLeg[3] = 182; |
salco | 0:e4151abdff23 | 198 | } |
salco | 0:e4151abdff23 | 199 | break; |
salco | 0:e4151abdff23 | 200 | } |
salco | 0:e4151abdff23 | 201 | } |
salco | 0:e4151abdff23 | 202 | } |
salco | 0:e4151abdff23 | 203 | } |
salco | 0:e4151abdff23 | 204 | void homemadeSequence::seqTurn(bool leftRIGHT,char idOperation,char idLeg) |
salco | 0:e4151abdff23 | 205 | {} |
salco | 0:e4151abdff23 | 206 | void homemadeSequence::seqWalk(bool backFRONT,char idOperation,char idLeg) |
salco | 0:e4151abdff23 | 207 | {} |
salco | 0:e4151abdff23 | 208 | void homemadeSequence::seqRepositioner(char idOperation,char idLeg) |
salco | 0:e4151abdff23 | 209 | {} |
salco | 0:e4151abdff23 | 210 | void homemadeSequence::seqCrabe(bool leftRIGHT,char idOperation,char idLeg) |
salco | 0:e4151abdff23 | 211 | {} |
salco | 0:e4151abdff23 | 212 | bool homemadeSequence::next_frame(void) |
salco | 0:e4151abdff23 | 213 | { |
salco | 0:e4151abdff23 | 214 | bool temp=true; |
salco | 0:e4151abdff23 | 215 | m_idFrame++; |
salco | 0:e4151abdff23 | 216 | unsigned char* tmpTable = get_frame(m_idFrame+1); |
salco | 0:e4151abdff23 | 217 | |
salco | 0:e4151abdff23 | 218 | if((tmpTable[0]==0) && (tmpTable[1]==0) && (tmpTable[2]==0) && (tmpTable[3]==0)) { |
salco | 0:e4151abdff23 | 219 | temp=false; |
salco | 0:e4151abdff23 | 220 | m_idFrame--;//get_frame(m_idFrame); |
salco | 0:e4151abdff23 | 221 | }/* else |
salco | 0:e4151abdff23 | 222 | m_idFrame++;*/ |
salco | 0:e4151abdff23 | 223 | |
salco | 0:e4151abdff23 | 224 | return temp; |
salco | 0:e4151abdff23 | 225 | } |
salco | 0:e4151abdff23 | 226 | bool homemadeSequence::prev_frame(void) |
salco | 0:e4151abdff23 | 227 | { |
salco | 0:e4151abdff23 | 228 | bool temp=true; |
salco | 0:e4151abdff23 | 229 | m_idFrame--; |
salco | 0:e4151abdff23 | 230 | unsigned char* tmpTable = get_frame(1); |
salco | 0:e4151abdff23 | 231 | |
salco | 0:e4151abdff23 | 232 | if((tmpTable[0]==0) && (tmpTable[1]==0) && (tmpTable[2]==0) && (tmpTable[3]==0)) { |
salco | 0:e4151abdff23 | 233 | temp=false; |
salco | 0:e4151abdff23 | 234 | m_idFrame++;//get_frame(m_idFrame); |
salco | 0:e4151abdff23 | 235 | } /*else |
salco | 0:e4151abdff23 | 236 | m_idFrame--;*/ |
salco | 0:e4151abdff23 | 237 | |
salco | 0:e4151abdff23 | 238 | return temp; |
salco | 0:e4151abdff23 | 239 | } |