Alexandre Salconi-Denis
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
Diff: source/Motrice/homemadeSequence.cpp
- Revision:
- 19:0ac80045ba4c
- Parent:
- 2:4c5b0f6d0a6b
- Child:
- 20:c825e7b07af7
--- a/source/Motrice/homemadeSequence.cpp Thu Apr 23 19:28:42 2015 +0000
+++ b/source/Motrice/homemadeSequence.cpp Mon Apr 27 20:41:21 2015 +0000
@@ -5,10 +5,47 @@
* Author: salco
*/
#include "homemadeSequence.h"
+unsigned char table_seqUp[9][2][3] = { { {80 , 210, 160}, {220, 90 , 114} },
+ { {80 , 220, 170}, {220, 80 , 130} },
+ { {90 , 220, 160}, {200, 80 , 140} },
+ { {106, 220, 150}, {194, 80 , 150} },
+ { {128, 220, 127}, {172, 80 , 173} },
+ { {144, 205, 130}, {156, 95 , 170} },
+ { {144, 210, 123}, {156, 89 , 177} },
+ { {172, 185, 124}, {128, 115, 176} },
+ { {194, 166, 118}, {106, 134, 182} } };
+
+unsigned char table_seqDown[9][2][3] = { { {172, 185, 124}, {128, 115, 176} },
+ { {172, 185, 124}, {128, 115, 176} },
+ { {144, 210, 123}, {156, 89 , 177} },
+ { {144, 205, 130}, {156, 95 , 170} },
+ { {128, 220, 127}, {172, 80 , 173} },
+ { {106, 220, 150}, {194, 80 , 150} },
+ { {90 , 220, 160}, {200, 80 , 140} },
+ { {80 , 220, 170}, {220, 80 , 130} },
+ { {80 , 100, 150}, {220, 200, 150} } };//fin de la seq il met ses patte sur sa tete**
+/*unsigned char table_seqUpDown[9][8][4] = { { {140, 80, 210, 160}, {165, 80, 210, 160}, {160, 80, 210, 160}, {150, 80, 210, 160},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} },
+ { {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4},
+ {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4}, {1, 2, 3, 4} } };*/
homemadeSequence::homemadeSequence()
{
-m_SequenceChosen=0;
-m_idFrame=0;
+ m_SequenceChosen=0;
+ m_idFrame=0;
}
homemadeSequence::~homemadeSequence()
{
@@ -16,11 +53,14 @@
}
unsigned char* homemadeSequence::get_frame(/*char idSequence,char idOperation,*/char idLeg)
{
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r In Get Frame");
m_posLeg[0] = 0;
m_posLeg[1] = 0;
m_posLeg[2] = 0;
m_posLeg[3] = 0;
-
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r idLeg: %i",idLeg);
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r m_SequenceChosen: %i",m_SequenceChosen);
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r m_idFrame: %i",m_idFrame);
switch(m_SequenceChosen) {
case 1:// 01 - Calibration(milieu) //
if(m_idFrame == 1) {//if(idOperation == 1) {
@@ -28,6 +68,11 @@
m_posLeg[1] = 150;
m_posLeg[2] = 150;
m_posLeg[3] = 150;
+ } else {
+ m_posLeg[0] = 0;
+ m_posLeg[1] = 0;
+ m_posLeg[2] = 0;
+ m_posLeg[3] = 0;
}
break;
@@ -52,13 +97,15 @@
case 10:// 10 - Crabe Droite //
break;
}
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r posLeg: [0]:%i, [1]:%i, [2]:%i, [3]:%i,",m_posLeg[0],m_posLeg[1],m_posLeg[2],m_posLeg[3]);
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r Out Get Frame");
return m_posLeg;
}
void homemadeSequence::seqUpDown(bool downUP,char idOperation,char idLeg)
{
if((idOperation<10)&&(idOperation>0)) {
- switch(idLeg) {
+ switch(idLeg) { //moving hips
case 1:
m_posLeg[0] = 140;
break;
@@ -78,7 +125,21 @@
m_posLeg[0] = 150;
break;
}
- if(idOperation == 1) {
+ if(downUP)
+ {
+ if(idLeg<5)
+ for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqUp[idOperation-1][0][i-1];
+ else
+ for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqUp[idOperation-1][1][i-1];
+ }
+ else
+ {
+ if(idLeg<5)
+ for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqDown[idOperation-1][0][i-1];
+ else
+ for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqDown[idOperation-1][1][i-1];
+ }
+ /*if(idOperation == 1) {
if(idLeg<5) {
m_posLeg[1] = 80;
m_posLeg[2] = 210;
@@ -106,9 +167,9 @@
}
switch(idOperation) {
- /*case 1:
+ case 1:
- break;*/
+ break;
case 2:
if(idLeg<5) {
m_posLeg[1] = 80;
@@ -197,8 +258,14 @@
m_posLeg[3] = 182;
}
break;
+ default:
+ m_posLeg[0] = 0;
+ m_posLeg[1] = 0;
+ m_posLeg[2] = 0;
+ m_posLeg[3] = 0;
+ break;
}
- }
+ }*/
}
}
void homemadeSequence::seqTurn(bool leftRIGHT,char idOperation,char idLeg)
@@ -211,29 +278,48 @@
{}
bool homemadeSequence::next_frame(void)
{
- bool temp=true;
+ debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r in Next Frame");
+ bool temp=false;
+ debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r m_idframe == %d",m_idFrame);
+
+ //if(m_idFrame > 1){
m_idFrame++;
- unsigned char* tmpTable = get_frame(m_idFrame+1);
-
- if((tmpTable[0]==0) && (tmpTable[1]==0) && (tmpTable[2]==0) && (tmpTable[3]==0)) {
- temp=false;
- m_idFrame--;//get_frame(m_idFrame);
- }/* else
- m_idFrame++;*/
+ unsigned char* tmpTable = get_frame(1);
+ if((tmpTable[0]!=0) && (tmpTable[1]!=0) && (tmpTable[2]!=0) && (tmpTable[3]!=0)) //{
+ temp=true;
+ //m_idFrame++;//get_frame(m_idFrame);
+ //}
+ else
+ m_idFrame--;
+
+ //}
+ //}
+ debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r m_idframe == %d",m_idFrame);
+ debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r out Next Frame");
return temp;
}
-bool homemadeSequence::prev_frame(void)
+bool homemadeSequence::prev_frame(void) //true == prev is ok
{
- bool temp=true;
- m_idFrame--;
- unsigned char* tmpTable = get_frame(1);
-
- if((tmpTable[0]==0) && (tmpTable[1]==0) && (tmpTable[2]==0) && (tmpTable[3]==0)) {
- temp=false;
- m_idFrame++;//get_frame(m_idFrame);
- } /*else
- m_idFrame--;*/
+ //false == impossible
+ debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r in Prev Frame");
+ bool temp=false;
+ debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r m_idframe == %d",m_idFrame);
+
+ if(m_idFrame > 1) {
+ m_idFrame--;
+ unsigned char* tmpTable = get_frame(1);
+ if((tmpTable[0]!=0) && (tmpTable[1]!=0) && (tmpTable[2]!=0) && (tmpTable[3]!=0)) //{
+ temp=true;
+ //m_idFrame++;//get_frame(m_idFrame);
+ //}
+ else
+ m_idFrame++;
+
+ //}
+ }
+ debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r m_idframe == %d",m_idFrame);
+ debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r out Prev Frame");
return temp;
}
\ No newline at end of file