Alexandre Salconi-Denis
/
ProjetOctopode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 27:ea60d12dccdf, committed 2015-05-14
- Comitter:
- salco
- Date:
- Thu May 14 21:40:23 2015 +0000
- Parent:
- 26:15cb45d56c28
- Child:
- 28:ac5c6350ed9a
- Commit message:
- Toute fonctione reste plus qua integrer les donner au projet.
Changed in this revision
--- a/directive.cpp Mon May 11 13:48:38 2015 +0000
+++ b/directive.cpp Thu May 14 21:40:23 2015 +0000
@@ -4,15 +4,21 @@
Directive::Directive():TTask(0)//on veux que cette tache sois exec toute les fois que TTaskGen sexecute.
{
c=0;
- ssc32 = new /*Serial*/RawSerial(PA_9, PA_10);
- //ctrDesPattes = new Faculter_motrice(ssc32/*pc*/);
+ ssc32 = new /*Serial*//*Raw*/Serial/*(USBTX, USBRX);*/(PB_6, PB_7);//(PA_9, PA_10);
+ ctrDesPattes = new Faculter_motrice(ssc32/*pc*/);
//m_CtrlBridge = m_CtrlBridge->getInstance();
//a enlever de commentaire//m_ListDesModules = m_CtrlBridge->findModule(0,1,0,0);
+
+ //Serial pc(USBTX, USBRX);
+
+ //ssc32->set_flow_control(0);
+ debug("\n\r directive Init");//printf("Hello World\n");
+ m_ListDesModules = m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0);
+ m_ListDesModules.append(m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0));
- // Serial pc(USBTX, USBRX);
-
- //ssc32->set_flow_control(0);
- ssc32->putc('G');//printf("Hello World\n");
+ m_capteurUltrasonic= m_CtrlBridge->findModule(0,CAPTEUR,ULTRASONIQUE,0x27/*0b100111*/);
+ m_capteurIR= m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0x27/*0b100111*/);
+ m_capteurProximiter= m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0x0E/*0b001110*/);
}
Directive::~Directive()
{
@@ -26,94 +32,29 @@
{
debug(DEBUG_DIRECTIVE_TEST,"\n\rIn task directive");
//pas sur que c'Est tout ce qui doit etre ici mais je vois pas quoi d'autre pour le moment.
- string flag,data,savedData;
- DigitalOut myled(LED1);
- DigitalIn mybutton(USER_BUTTON);
-
-
- //wait(1);// a enlever plus tard pour gain de vitesse//
- ////////////////
-// Inspection //
-////////////////
-
- savedData.clear();
-
- /* for(int i=0; i<m_ListDesModules.length(); ++i) {
- flag.clear();
- data.clear();
- debug("\n\r result: %d",m_CtrlBridge->send(m_ListDesModules.at(i),flag,data));
- if(data[0]!= 0x00)
- savedData.append(data);
- }*/
- int buttonCount;
- for(buttonCount =0; mybutton == 0; buttonCount++){
- debug(DEBUG_DIRECTIVE_TEST,"\n\r %02i",buttonCount+1);
- wait(1);
- myled = 1;
- wait_ms(500);
- myled = 0;
- }
-debug(DEBUG_DIRECTIVE_TEST,"\n\rResult : %02i",buttonCount);
-
-//////////////////////////////
-// Traitement du Labyrinthe //
-//////////////////////////////
+ string flag,data;
- switch(buttonCount){
- case 2:
- ctrDesPattes->calibre(1);
- ctrDesPattes->calibre(2);
- ctrDesPattes->exec();
- break;
-
- case 6:
- ctrDesPattes->stop();
- ctrDesPattes->exec();
- ctrDesPattes->resume();
- break;
-
- case 3:
- ctrDesPattes->moveUp();
- ctrDesPattes->exec();
- break;
-
- case 4:
- ctrDesPattes->moveDown();
- ctrDesPattes->exec();
- break;
-
- case 5:
- //ctrDesPattes->stop();
- //ctrDesPattes->exec();
- ctrDesPattes->resume();
- break;
-
- case 1:
- //ctrDesPattes->stop();
- ctrDesPattes->exec();
- //ctrDesPattes->resume();
- break;
-
- case 7:
- ctrDesPattes->moveLeft();
- ctrDesPattes->exec();
- default:
- break;
- }
-
- /* if(savedData.size() != 0)
+ ////////////////
+ // Inspection //
+ ////////////////
+ flag.append(1,0);//0x02);
+ for(int i=0; i<m_ListDesModules.length(); ++i)
{
- ctrDesPattes->exec();
- }*/
- /* if (c == 'g') {
- ctrDesPattes->calibre();
- c=0;
- }
- if(c == 'h') {
- pc->printf(" ID seq: %i \n\r",ctrDesPattes->get_idSeq());
- c=0;
- }*/
-///////////////
+ wait_us(300);
+ flag.clear();
+ flag.append(1,7);
+ data.clear();
+ debug("\n\r result: %d",m_CtrlBridge->send(m_ListDesModules.at(i),flag,data));
+
+ debug("\n\r flag: %d",flag[0]);
+ debug("\n\r data: %x",data[0]);
+
+ }
+ //////////////////////////////
+ // Traitement du Labyrinthe //
+ //////////////////////////////
+
+ ///////////////
// Mouvement //
///////////////
// ctrDesPattes.exec();
@@ -124,7 +65,5 @@
////////////
// Autre? //
////////////
-
-debug(DEBUG_DIRECTIVE_TEST,"\n\rOut task directive");
-
+ debug(DEBUG_DIRECTIVE_TEST,"\n\rOut task directive");
}
\ No newline at end of file
--- a/directive.h Mon May 11 13:48:38 2015 +0000
+++ b/directive.h Thu May 14 21:40:23 2015 +0000
@@ -17,9 +17,10 @@
class Directive : public TTask
{
- CtrlBridge* m_CtrlBridge;
+ //CtrlBridge* m_CtrlBridge;
Faculter_motrice *ctrDesPattes;
- /*Serial*/RawSerial *ssc32;
+ /*Serial*//*Raw*/Serial *ssc32;
+ string m_capteurUltrasonic,m_capteurIR,m_capteurProximiter;
char c;
protected:
virtual void task(void);
--- a/main.cpp Mon May 11 13:48:38 2015 +0000
+++ b/main.cpp Thu May 14 21:40:23 2015 +0000
@@ -3,7 +3,7 @@
//#include "OSNAPprotocoleDefine.h"
//#include "mouvement.h"
#include "debug.h"
-//#include "Concience/InstinctPrimaire.h"
+
#include "source/Task/TTaskCritique.h"
#include "source/Task/TTaskAlerte.h"
#include "source/Task/TTaskGeneral.h"
@@ -21,24 +21,9 @@
DigitalIn mybutton(USER_BUTTON);
DigitalOut pinA(PA_13);
DigitalOut pinB(PA_14);
-Serial patate (PB_6, PA_10);
+
//Serial pc(PB_10,PB_11);
-UART_HandleTypeDef huart3;
-void MX_USART3_UART_Init(void)
-{
-
- huart3.Instance = USART3;
- huart3.Init.BaudRate = 9600;
- huart3.Init.WordLength = UART_WORDLENGTH_8B;
- huart3.Init.StopBits = UART_STOPBITS_1;
- huart3.Init.Parity = UART_PARITY_NONE;
- huart3.Init.Mode = UART_MODE_TX_RX;
- huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
- huart3.Init.OverSampling = UART_OVERSAMPLING_16;
- HAL_UART_Init(&huart3);
-
-}
int main()
{
@@ -49,28 +34,28 @@
// string test="allo";
// pc.printf(test.c_str());
- //TTaskCritique taskCritique(1);
+ TTaskCritique taskCritique(40);
// TTaskAlerte taskAlerte(20);
- // TTaskGeneral taskGeneral(1);//40);
+ TTaskGeneral taskGeneral(1);//40);
+wait(1);
//////////
// Boot //
//////////
- MX_USART3_UART_Init();
- uint8_t pat[5]= {'A','l','l','o',0};
while(1) {
- HAL_UART_Transmit(&huart3,pat,5,5);
+
//taskAlerte.exec();
//taskGeneral.exec();
if (mybutton == 0) { // Button is pressed
//debug("Hello debug world");
- //taskCritique.exec();
- patate.putc(64);
+ taskCritique.exec();
+ wait(2);
+ taskGeneral.exec();
- //taskGeneral.exec();
+
/*if(myled) {
pinB=0;
pinA=1;
@@ -89,6 +74,6 @@
delete taskCritt;
if(taskAlert)
delete taskAlert;
- if(taskGen)
- delete taskGen;*/
+ if(taskGeneral)
+ delete taskGeneral;*/
}
--- a/source/ComSpi.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/ComSpi.cpp Thu May 14 21:40:23 2015 +0000
@@ -144,7 +144,7 @@
if(portID < 1) portID = 0;
else if(portID > 16) portID = 16;
- debug(DEBUG_SEND, "\n\r -Change PORT. ");
+ debug(DEBUG_SEND| DEBUG_COMPACT, "\n\r -Change PORT. %02d",portID);
m_demuxPos = portID;
change_demux();
@@ -234,7 +234,7 @@
for(int retryLoop=0; (retryLoop<3) && (!result); retryLoop++) {
-
+ wait_us(3000);
debug(DEBUG_SEND|DEBUG_COMPACT, "\n\r -Tentative: %d \n\r",retryLoop);
formatedDataReceive.clear();
settingSlave =0;
@@ -252,12 +252,13 @@
for(int i = 0; (i < formatedDataSend.length()) && (i<=(2+(settingMaster>>6))); ++i) {
formatedDataReceive.append(1,write(formatedDataSend[i]));//debug(DEBUG_SEND|DEBUG_COMPACT|DEBUG_SLIM, "\n\r -A:%i ",i);
debug(DEBUG_SEND |DEBUG_COMPACT, " %02X,",formatedDataSend.at(i));
+ wait_us(3000);
}
if(formatedDataReceive[0] == SYNC) {
debug(DEBUG_COMPACT, "\n\r -Receve transmission. ");
//!!!!!!!! Problem ici write dans un mais pas lautre
- wait_us(3000);
+ //wait_us(3000);
if(retryLoop == 0) {
formatedDataSend.append(1,0);//2,0); cest une patch tempo parce que je trouve pas ou il manque la donner
}
@@ -266,6 +267,7 @@
settingSlave = formatedDataReceive[formatedDataReceive.length()-1];
debug(DEBUG_COMPACT, "\n\r -settingSlave %02X",settingSlave);
+ debug(DEBUG_COMPACT, "\n\r -append %02X time(s)",settingSlave>>6);
if(retryLoop == 0) {
/*switch(formatedDataReceive[formatedDataSend.length()-1] >> 6) {
case 1:
@@ -285,7 +287,6 @@
// break;
}*/
formatedDataSend.append(settingSlave>>6,0);
- debug(DEBUG_COMPACT, "\n\r -append %02X time(s)",settingSlave>>6);
}
int bufferFlag = 0;
@@ -299,16 +300,23 @@
counterTotale = ((2+(settingMaster>>6))+1);
counterTotale = counterTotale+(settingSlave>>6)+abs((settingMaster & 0x0F) - (settingSlave & 0x0F));
+ debug(DEBUG_COMPACT, "\n\r -Calcule: %02X=((2+(%02X))+1)+(%02X)+abs((%02X)-(%02X))",(((2+(settingMaster>>6))+1)+(settingSlave>>6)+abs((settingMaster & 0x0F) - (settingSlave & 0x0F))),(settingMaster>>6),(settingSlave>>6),(settingMaster & 0x0F),(settingSlave & 0x0F));
//debug( "\n\r -size : %02X",counterTotale);
-
+ debug( "\n\r -size : %02X , %02X",formatedDataSend.size(),counterTotale);
if(retryLoop == 0) {
- debug( "\n\r -size : %02X , %02X",formatedDataSend.size(),counterTotale);
- if(formatedDataSend.size()<counterTotale) {
+
+ if(formatedDataSend.size()<=counterTotale) {
formatedDataSend.append(counterTotale+1-formatedDataSend.size(),0);
}
+ else{
+ debug(DEBUG_SEND|DEBUG_COMPACT|DEBUG_SLIM, "\n\r -Probleme de receive: ");
+ for (unsigned i=0; i<formatedDataReceive.length(); i++) debug(DEBUG_SEND|DEBUG_COMPACT|DEBUG_SLIM, "%02X,",formatedDataReceive.at(i));
+ debug(DEBUG_SEND|DEBUG_COMPACT|DEBUG_SLIM, "\n\r -Probleme de send: ");
+ for (unsigned i=0; i<formatedDataSend.length(); i++) debug(DEBUG_SEND|DEBUG_COMPACT|DEBUG_SLIM, "%02X,",formatedDataSend.at(i));
+ }
}
-
+ debug( "\n\r -size : %02X , %02X",formatedDataSend.size(),counterTotale);
switch(bufferFlag) { //plus facile pour savoir ce que tu doit tatenre a recevoire
case 1://Request Init Info
if(retryLoop == 0) {
--- a/source/CtrlBridge.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/CtrlBridge.cpp Thu May 14 21:40:23 2015 +0000
@@ -16,7 +16,7 @@
m_regPortUse=0;
m_regPortLost=0;
spiLowSpeed.format(8,0);
- spiLowSpeed.frequency(10000);
+ spiLowSpeed.frequency(10000/**/);
}
CtrlBridge::~CtrlBridge()
@@ -148,7 +148,11 @@
if(m_Memory.isAdresseValide(adresse,moduleRequested)) {
if(adresse&0x80)
+ {
+ debug(DEBUF_SEND, "\n\rPointeur SPI: %d",&spiLowSpeed);
+ //debug(DEBUF_SEND, "\n\rPointeur SPI: %p",spiLowSpeed);
result = spiLowSpeed.send(moduleRequested.regB>>4,adresse,&flag,&data);
+ }
else
result = spiHighSpeed.send(moduleRequested.regB>>4,adresse,&flag,&data);
//result = true;
@@ -172,14 +176,27 @@
debug(DEBUF_FINDMODULE, "\n\r -Debut scan Actioneur");
maxSize = m_Memory.getSizeActioneur();
+ debug(DEBUF_FINDMODULE, "\n\r -MaxSize: %02d",maxSize);
goodModule = true;
moduleScan= m_Memory.firstActioneur();
for(int i=0; i < maxSize; i++) {
- if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)) goodModule =false;
- else if((type != 0) && (type != moduleScan.regD>>6)) goodModule =false;
- else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))) goodModule =false;
- else if((posSpatial != 0) && (sousType != (moduleScan.regC & 0x3F))) goodModule =false;
+ if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -peripheriqueID: %02d == moduleScan.regB>>4: %02d",peripheriqueID,moduleScan.regB>>4);
+ }
+ else if((type != 0) && (type != moduleScan.regD>>6)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -type: %02d == moduleScan.regD>>6: %02d",type,moduleScan.regD>>6);
+ }
+ else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -sousType: %02d == (moduleScan.regD & 0x3F): %02d",sousType,(moduleScan.regD & 0x3F));
+ }
+ else if((posSpatial != 0) && (posSpatial != (moduleScan.regC & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -posSpatial: %02d == (moduleScan.regC & 0x3F): %02d",posSpatial,(moduleScan.regC & 0x3F));
+ }
if(goodModule){
result.append(1,moduleScan.regA);
@@ -194,14 +211,34 @@
debug(DEBUF_FINDMODULE, "\n\r -Debut scan Capteur");
maxSize = m_Memory.getSizeCapteur();
- goodModule = true;
+ debug(DEBUF_FINDMODULE, "\n\r -MaxSize: %02d",maxSize);
+
moduleScan= m_Memory.firstCapteur();
for(int i=0; i < maxSize; i++) {
- if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)) goodModule =false;
- else if((type != 0) && (type != moduleScan.regD>>6)) goodModule =false;
- else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))) goodModule =false;
- else if((posSpatial != 0) && (sousType != (moduleScan.regC & 0x3F))) goodModule =false;
+ goodModule = true;
+ debug(DEBUF_FINDMODULE, "\n\r -adresse: %02x",moduleScan.regA);
+ debug(DEBUF_FINDMODULE, "\n\r -peripheriqueID: %02d == moduleScan.regB>>4: %02d",peripheriqueID,moduleScan.regB>>4);
+ debug(DEBUF_FINDMODULE, "\n\r -type: %02d == moduleScan.regD>>6: %02d",type,moduleScan.regD>>6);
+ debug(DEBUF_FINDMODULE, "\n\r -sousType: %02d == (moduleScan.regD & 0x3F): %02d",sousType,(moduleScan.regD & 0x3F));
+ debug(DEBUF_FINDMODULE, "\n\r -posSpatial: %02d == (moduleScan.regC & 0x3F): %02d",posSpatial,(moduleScan.regC & 0x3F));
+
+ if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -false");
+ }
+ else if((type != 0) && (type != moduleScan.regD>>6)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -false");
+ }
+ else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -false");
+ }
+ else if((posSpatial != 0) && (posSpatial != (moduleScan.regC & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -false");
+ }
if(goodModule){
result.append(1,moduleScan.regA);
@@ -216,14 +253,27 @@
debug(DEBUF_FINDMODULE, "\n\r -Debut scan Memoire");
maxSize = m_Memory.getSizeMemoire();
+ debug(DEBUF_FINDMODULE, "\n\r -MaxSize: %02d",maxSize);
goodModule = true;
moduleScan= m_Memory.firstMemoire();
for(int i=0; i < maxSize; i++) {
- if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)) goodModule =false;
- else if((type != 0) && (type != moduleScan.regD>>6)) goodModule =false;
- else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))) goodModule =false;
- else if((posSpatial != 0) && (sousType != (moduleScan.regC & 0x3F))) goodModule =false;
+ if((peripheriqueID != 0) && (peripheriqueID != moduleScan.regB>>4)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -peripheriqueID: %02d == moduleScan.regB>>4: %02d",peripheriqueID,moduleScan.regB>>4);
+ }
+ else if((type != 0) && (type != moduleScan.regD>>6)){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -type: %02d == moduleScan.regD>>6: %02d",type,moduleScan.regD>>6);
+ }
+ else if((sousType != 0) && (sousType != (moduleScan.regD & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -sousType: %02d == (moduleScan.regD & 0x3F): %02d",sousType,(moduleScan.regD & 0x3F));
+ }
+ else if((posSpatial != 0) && (posSpatial != (moduleScan.regC & 0x3F))){
+ goodModule =false;
+ debug(DEBUF_FINDMODULE, "\n\r -posSpatial: %02d == (moduleScan.regC & 0x3F): %02d",posSpatial,(moduleScan.regC & 0x3F));
+ }
if(goodModule){
result.append(1,moduleScan.regA);
--- a/source/MemRegistre.h Mon May 11 13:48:38 2015 +0000 +++ b/source/MemRegistre.h Thu May 14 21:40:23 2015 +0000 @@ -9,7 +9,7 @@ #define MEMREG_H #define DEBUG_GETADDR 1 -#define DEBUG_ISVALIDE 1 +#define DEBUG_ISVALIDE 0 #include <list> #include "Module.h"
--- a/source/Motrice/Patte.h Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/Patte.h Thu May 14 21:40:23 2015 +0000
@@ -14,6 +14,7 @@
//#include <string>
#include <stdio.h>
+//#include "debug.h"
struct servo_t {
char iD; //use for ssc-32
@@ -38,52 +39,87 @@
servo_t m_Tibia;
servo_t m_Tars;
char buffer [50];
-
+ bool valuesChange;
+ bool valuesCoxaChange;
+ bool valuesFemurChange;
+ bool valuesTibiaChange;
+ bool valuesTarsChange;
+
public:
Patte(char patteId,char coxaId,char femurId,char tibiaId,char tarsId) {
m_id = patteId;
+ valuesChange = false;
m_Coxa.iD = coxaId;
m_Coxa.max = DEFAULTMAX;
m_Coxa.min = DEFAULTMIN;
m_Coxa.curr= DEFAULTPOSITION;
+ valuesCoxaChange = false;
m_Femur.iD = femurId;
m_Femur.max = DEFAULTMAX;
m_Femur.min = DEFAULTMIN;
m_Femur.curr= DEFAULTPOSITION;
+ valuesFemurChange = false;
m_Tibia.iD = tibiaId;
m_Tibia.max = DEFAULTMAX;
m_Tibia.min = DEFAULTMIN;
m_Tibia.curr= DEFAULTPOSITION;
+ valuesTibiaChange = false;
m_Tars.iD = tarsId;
m_Tars.max = DEFAULTMAX;
m_Tars.min = DEFAULTMIN;
m_Tars.curr= DEFAULTPOSITION;
+ valuesTarsChange = false;
}
- virtual ~Patte(){}
+ virtual ~Patte() {}
char id(void) {
return m_id;
}
- const char* toString(void){
-
- sprintf (buffer,"#%d P%d0 #%d P%d0 #%d P%d0 #%d P%d0",m_Coxa.iD,m_Coxa.curr,m_Femur.iD,m_Femur.curr,m_Tibia.iD,m_Tibia.curr,m_Tars.iD,m_Tars.curr);
- return buffer;
- }
+ const char* toString(void) {
+ char buffer1[50]= {""};
+ char buffer2[50]= {""};
+ char buffer3[50]= {""};
+ char buffer4[50]= {""};
+ for(int i=0;i<50;i++)buffer[i]=0;
+
+ if(valuesCoxaChange)
+ sprintf (buffer1,"#%d P%d0 ",m_Coxa.iD,m_Coxa.curr);
+ if(valuesFemurChange)
+ sprintf (buffer2,"#%d P%d0 ",m_Femur.iD,m_Femur.curr);
+ if(valuesTibiaChange)
+ sprintf (buffer3,"#%d P%d0 ",m_Tibia.iD,m_Tibia.curr);
+ if(valuesTarsChange)
+ sprintf (buffer4,"#%d P%d0",m_Tars.iD,m_Tars.curr);
+ //sprintf (buffer,"#%d P%d0 #%d P%d0 #%d P%d0 #%d P%d0",m_Coxa.iD,m_Coxa.curr,m_Femur.iD,m_Femur.curr,m_Tibia.iD,m_Tibia.curr,m_Tars.iD,m_Tars.curr);
+ sprintf (buffer,"%s%s%s%s",buffer1,buffer2,buffer3,buffer4);
+ //debug(1,"\n\n\rintern string %s",buffer);
+
+ return buffer;
+ }
+ bool haveChange() {
+ bool result = valuesChange|valuesCoxaChange|valuesFemurChange|valuesTibiaChange|valuesTarsChange;
+ if(valuesChange)valuesChange=false;
+ return result;
+ }
//setting Coxa //
void set_Id_Coxa(char id) {
m_Coxa.iD = id;
+ valuesChange =true;
}
void set_PulseMax_Coxa(unsigned char value) {
m_Coxa.max = value;
+ valuesChange =true;
}
void set_PulseMin_Coxa(unsigned char value) {
m_Coxa.min = value;
+ valuesChange =true;
}
void set_DefaultPulse_Coxa(unsigned char value) {
m_Coxa.basePose = value;
+ valuesChange =true;
}
char get_Id_Coxa(void) {
return m_Coxa.iD;
@@ -103,15 +139,19 @@
//setting Femur //
void set_Id_Femur(char id) {
m_Femur.iD = id;
+ valuesChange =true;
}
void set_PulseMax_Femur(unsigned char value) {
m_Femur.max = value;
+ valuesChange =true;
}
void set_PulseMin_Femur(unsigned char value) {
m_Femur.min = value;
+ valuesChange =true;
}
void set_DefaultPulse_Femur(unsigned char value) {
m_Femur.basePose = value;
+ valuesChange =true;
}
char get_Id_Femur(void) {
return m_Femur.iD;
@@ -131,15 +171,19 @@
//setting Tibia //
void set_Id_Tibia(char id) {
m_Tibia.iD = id;
+ valuesChange =true;
}
void set_PulseMax_Tibia(unsigned char value) {
m_Tibia.max = value;
+ valuesChange =true;
}
void set_PulseMin_Tibia(unsigned char value) {
m_Tibia.min = value;
+ valuesChange =true;
}
void set_DefaultPulse_Tibia(unsigned char value) {
m_Tibia.basePose = value;
+ valuesChange =true;
}
char get_Id_Tibia(void) {
return m_Tibia.iD;
@@ -159,15 +203,19 @@
//setting Tars //
void set_Id_Tars(char id) {
m_Tars.iD = id;
+ valuesChange =true;
}
void set_PulseMax_Tars(unsigned char value) {
m_Tars.max = value;
+ valuesChange =true;
}
void set_PulseMin_Tars(unsigned char value) {
m_Tars.min = value;
+ valuesChange =true;
}
void set_DefaultPulse_Tars(unsigned char value) {
m_Tars.basePose = value;
+ valuesChange =true;
}
char get_Id_Tars(void) {
return m_Tars.iD;
@@ -190,6 +238,7 @@
m_Femur.iD= femurId;
m_Tibia.iD= tibiaId;
m_Tars.iD = tarsId;
+ valuesChange =true;
}
void set_PulseMax(unsigned char coxaValue,unsigned char femurValue,
unsigned char tibiaValue,unsigned char tarsValue) {
@@ -197,6 +246,7 @@
m_Femur.max= femurValue;
m_Tibia.max= tibiaValue;
m_Tars.max = tarsValue;
+ valuesChange =true;
}
void set_PulseMin(unsigned char coxaValue,unsigned char femurValue,
unsigned char tibiaValue,unsigned char tarsValue) {
@@ -204,6 +254,7 @@
m_Femur.min= femurValue;
m_Tibia.min= tibiaValue;
m_Tars.min = tarsValue;
+ valuesChange =true;
}
void set_DefaultPulse(unsigned char coxaValue,unsigned char femurValue,
unsigned char tibiaValue,unsigned char tarsValue) {
@@ -211,36 +262,49 @@
m_Femur.basePose= femurValue;
m_Tibia.basePose= tibiaValue;
m_Tars.basePose = tarsValue;
+ valuesChange =true;
}
//////////
// Move //
//////////
void move_Coxa(unsigned char value) {
- if((value >= m_Coxa.min)&&(value <= m_Coxa.max ))
+ if((value >= m_Coxa.min)&&(value <= m_Coxa.max )&& (value != 255) ) {
m_Coxa.curr = value;
+ valuesCoxaChange =true;
+ }
+ else
+ valuesCoxaChange =false;
}
void move_Femur(unsigned char value) {
- if((value >= m_Femur.min)&&(value <= m_Femur.max ))
+ if((value >= m_Femur.min)&&(value <= m_Femur.max )&& (value != 255)) {
m_Femur.curr = value;
+ valuesFemurChange =true;
+ }
+ else
+ valuesFemurChange =false;
}
void move_Tibia(unsigned char value) {
- if((value >= m_Tibia.min)&&(value <= m_Tibia.max ))
+ if((value >= m_Tibia.min)&&(value <= m_Tibia.max )&& (value != 255)) {
m_Tibia.curr = value;
+ valuesTibiaChange =true;
+ }
+ else
+ valuesTibiaChange =false;
}
void move_Tars(unsigned char value) {
- if((value >= m_Tars.min)&&(value <= m_Tars.max ))
+ if((value >= m_Tars.min)&&(value <= m_Tars.max )&& (value != 255)) {
m_Tars.curr = value;
+ valuesTarsChange =true;
+ }
+ else
+ valuesTarsChange =false;
}
void move(unsigned char coxaValue,unsigned char femurValue,
unsigned char tibiaValue,unsigned char tarsValue) {//noob proof
- if((coxaValue >= m_Coxa.min)&&(coxaValue <= m_Coxa.max ))
- m_Coxa.curr = coxaValue;
- if((femurValue >= m_Femur.min)&&(femurValue <= m_Femur.max ))
- m_Femur.curr = femurValue;
- if((tibiaValue >= m_Tibia.min)&&(tibiaValue <= m_Tibia.max ))
- m_Tibia.curr = tibiaValue;
- if((tarsValue >= m_Tars.min)&&(tarsValue <= m_Tars.max ))
- m_Tars.curr = tarsValue;
+ this->move_Coxa(coxaValue);
+ this->move_Femur(femurValue);
+ this->move_Tibia(tibiaValue);
+ this->move_Tars(tarsValue);
}
};
#endif /* PATTE_H_ */
\ No newline at end of file
--- a/source/Motrice/homemadeSequence.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/homemadeSequence.cpp Thu May 14 21:40:23 2015 +0000
@@ -5,7 +5,19 @@
* Author: salco
*/
#include "homemadeSequence.h"
-unsigned char table_seqUp[9][2][3] = { { {80 , 210, 160}, {220, 90 , 114} },
+
+
+unsigned char table_seqDefaultPos[8][4] = {
+ {137,123,217,135},
+ {165,120,222,135},
+ {160,117,217,137},
+ {150,111,213,142},
+ {153,181, 83,166},
+ {133,187, 78,161},
+ {170,192, 78,156},
+ {144,181, 81,165}};
+#define SIZE_SEQUP 9
+unsigned char table_seqUp[SIZE_SEQUP][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} },
@@ -16,7 +28,7 @@
{ {194, 166, 118}, {106, 134, 182} }
};
-unsigned char table_seqDown[9][2][3] = { { {172, 185, 124}, {128, 115, 176} },
+unsigned char table_seqDown[SIZE_SEQUP][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} },
@@ -27,7 +39,8 @@
{ {80 , 100, 150}, {220, 200, 150} }
};//fin de la seq il met ses patte sur sa tete**
-unsigned char table_seqCrabe[33][8][3] = {{ /* 0*/{255,255,255}, /* 4*/{125, 184, 156}, /* 8*/{132, 215, 147}, /*12*/{255,255,255},
+#define SIZE_SEQCRABE 33
+unsigned char table_seqCrabe[SIZE_SEQCRABE][8][3] = {{ /* 0*/{255,255,255}, /* 4*/{125, 184, 156}, /* 8*/{132, 215, 147}, /*12*/{255,255,255},
/*16*/{203,101,128}, /*20*/{255,255,255}, /*24*/{176, 87, 163}, /*28*/{191, 103,133}
},
{ /* 0*/{255,255,255}, /* 4*/{111, 185, 156}, /* 8*/{255, 255, 255}, /*12*/{255, 255,255},
@@ -127,13 +140,16 @@
/*16*/{203, 95,110}, /*20*/{255, 255, 255}, /*24*/{165, 109, 152}, /*28*/{191, 101,120}
}
};
-
-unsigned char table_seqTurn_PAUSE_TIME[32][2] = {{ 30, 30},{ 20, 20},{ 25, 25},{ 20, 30},{ 25, 20},{ 21, 30},{ 30, 30},
- { 30, 30},{ 20, 20},{ 20, 20},{ 20, 30},{ 20, 20},{ 20, 20},{ 30, 30},{ 31, 30},
- { 20, 20},{ 30, 30},{ 20, 30},{ 30, 30},{ 20, 20},{ 20, 30},{ 30, 30},{ 30, 30}
+#define SIZE_SEQTURN 31
+unsigned char table_seqTurn_PAUSE_TIME[SIZE_SEQTURN][2] = {
+ { 30, 30},{ 20, 20},{ 25, 25},{ 20, 30},{ 20, 20},{ 30, 30},{ 30, 30},{ 30, 30},
+ { 30, 30},{ 20, 20},{ 25, 25},{ 20, 30},{ 25, 20},{ 21, 30},{ 30, 30},{ 30, 30},
+ { 20, 20},{ 20, 20},{ 20, 30},{ 20, 20},{ 20, 20},{ 30, 30},{ 31, 30},
+ { 20, 20},{ 30, 30},{ 20, 30},{ 30, 30},{ 20, 20},{ 20, 30},{ 30, 30},{ 0, 30}
};
-unsigned char table_seqTurn[31][8][3] = {{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{ 92,211,171}, /*12*/{255,255,255},
+unsigned char table_seqTurn[SIZE_SEQTURN][8][3] = {
+ { /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{ 92,211,171}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{213, 85,118}, /*24*/{255,255,255}, /*28*/{255,255,255}
},
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{ 89,193,186}, /*12*/{255,255,255},
@@ -172,10 +188,10 @@
{ /* 0*/{ 93,187,203}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{203,103,115}
},
- { /* 0*/{123,220,139}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
+ { /* 0*/{123,220,139}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{178, 78,169}
},
- { /* 0*/{123,217,215}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
+ { /* 0*/{123,217,135}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{181, 81,167}
},
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{ 99,211,176},
@@ -227,8 +243,10 @@
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
}
};
-unsigned char table_seqRepositionne_PAUSE_TIME[6][2] = {{ 11, 10},{ 16, 15},{ 15, 15},{ 21, 20},{ 21, 20},{ 51, 50}};
-unsigned char table_seqRepositionne[6][8][3] = {
+
+#define SIZE_SEQREPOSITIONNE 6
+unsigned char table_seqRepositionne_PAUSE_TIME[SIZE_SEQREPOSITIONNE][2] = {{ 11, 10},{ 16, 15},{ 15, 15},{ 21, 20},{ 21, 20},{ 51, 50}};
+unsigned char table_seqRepositionne[SIZE_SEQREPOSITIONNE][8][3] = {
{ /* 0*/{103,206,176}, /* 4*/{102,213,176}, /* 8*/{ 92,211,171}, /*12*/{ 99,211,176},
/*16*/{210, 86,121}, /*20*/{213, 85,118}, /*24*/{217, 83,118}, /*28*/{200, 86,124}
},
@@ -248,15 +266,16 @@
/*16*/{181, 83,166}, /*20*/{187, 78,161}, /*24*/{194, 78,161}, /*28*/{181, 81,167}
}
};
-unsigned char table_seqFoward_PAUSE_TIME[42][2] = {
+#define SIZE_SEQFOWARD 43
+unsigned char table_seqFoward_PAUSE_TIME[SIZE_SEQFOWARD][2] = {
{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 21, 20},{ 0, 45},{ 61, 50},
{ 21, 20},{ 51, 50},{ 21, 20},{ 21, 20},{ 31, 30},{ 51, 50},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},
- { 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 51, 50},{ 21, 20},{ 41, 45},
+ { 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 51, 50},{ 0, 50},{ 21, 20},{ 41, 45},
{ 11, 10},{ 16, 15},{ 15, 15},{ 21, 20},{ 21, 20},{ 51, 50},
{ 15, 15},{ 21, 20},{ 21, 20},{ 51, 50},
{ 15, 15},{ 21, 20},{ 21, 20},{ 51, 50}
};
-unsigned char table_seqFoward[42][8][3] = {
+unsigned char table_seqFoward[SIZE_SEQFOWARD][8][3] = {
{ /* 0*/{103,206,176}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{213, 85,118}, /*24*/{255,255,255}, /*28*/{255,255,255}
},
@@ -335,6 +354,9 @@
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{145,158,173},
/*16*/{179, 82,167}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
},
+ { /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
+ /*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
+ },
{ /* 0*/{255,255,255}, /* 4*/{120,222,135}, /* 8*/{255,255,255}, /*12*/{111,213,142},//attention plusieur temps
/*16*/{181, 83,166}, /*20*/{255,255,255}, /*24*/{182, 78,161}, /*28*/{255,255,255}
},
@@ -384,13 +406,15 @@
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
}
};
-unsigned char table_seqBackward_PAUSE_TIME[33][2] = {
- { 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 31, 30},{ 61, 50},
+
+#define SIZE_SEQBACKWARD 35
+unsigned char table_seqBackward_PAUSE_TIME[SIZE_SEQBACKWARD][2] = {
+ { 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 31, 30},{ 0, 45},{ 61, 50},
{ 21, 20},{ 21, 20},{ 31, 30},{ 51, 50},{ 21, 20},{ 21, 20},{ 31, 30},{ 51, 50},{ 16, 15},{ 21, 30},
- { 50,100},{ 21, 20},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 51, 45},{ 21, 20},{ 41, 45},{ 21, 20},
+ { 50,100},{ 21, 20},{ 16, 15},{ 21, 30},{ 50,100},{ 21, 20},{ 51, 45},{ 0, 50},{ 21, 20},{ 41, 45},{ 21, 20},
{ 21, 20},{ 31, 30},{ 31, 50}
};
-unsigned char table_seqBackward[33][8][3] = {
+unsigned char table_seqBackward[SIZE_SEQBACKWARD][8][3] = {
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{ 92,211,171}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{200, 86,124}
},
@@ -422,6 +446,9 @@
/*16*/{255,255,255}, /*20*/{187, 78,161}, /*24*/{255,255,255}, /*28*/{155,137,130}
},
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
+ /*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
+ },
+ { /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{206,108,127}
},
{ /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
@@ -472,6 +499,9 @@
{ /* 0*/{255,255,255}, /* 4*/{125,225,133}, /* 8*/{255,255,255}, /*12*/{255,255,255},
/*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{180, 86,156}, /*28*/{255,255,255}
},
+ { /* 0*/{255,255,255}, /* 4*/{255,255,255}, /* 8*/{255,255,255}, /*12*/{255,255,255},
+ /*16*/{255,255,255}, /*20*/{255,255,255}, /*24*/{255,255,255}, /*28*/{255,255,255}
+ },
{ /* 0*/{255,255,255}, /* 4*/{120,222,135}, /* 8*/{255,255,255}, /*12*/{111,213,142},//attention plusieur temps
/*16*/{181, 83,166}, /*20*/{255,255,255}, /*24*/{182, 78,161}, /*28*/{255,255,255}
},
@@ -514,11 +544,49 @@
m_SequenceChosen=0;
m_idFrame=0;
m_idLegUse =0;
+ delaisNeed =-1;
+ for(int i=0; i<8; i++)timeSequence[i]=255;
}
homemadeSequence::~homemadeSequence()
{
}
+int homemadeSequence::patchDelais(char idMode)
+{
+ int result=-1;
+ switch(idMode) {
+ /*case 1:
+ break;
+ case 2:
+ break;
+ case 3:
+ break;*/
+ case 4:
+ result = (table_seqTurn_PAUSE_TIME[0][0])*10;
+ break;
+ /*case 5:
+ break;*/
+ case 6:
+ result = (table_seqFoward_PAUSE_TIME[0][0])*10;
+ break;
+ case 7:
+ result = (table_seqBackward_PAUSE_TIME[0][0])*10;
+ break;
+ case 8:
+ result = (table_seqRepositionne_PAUSE_TIME[0][0])*10;
+ break;
+ /*case 9:
+ break;
+ case 10:
+ break;
+ case 11:
+ break;*/
+ default:
+ result = -1;
+ break;
+ }
+ return result;
+}
unsigned char* homemadeSequence::get_frame(/*char idSequence,char idOperation,*/char idLeg)
{
debug(DEBUG_HOMEMADE_GETFRAME,"\n\r In Get Frame");
@@ -551,9 +619,13 @@
seqUpDown(false,m_idFrame,idLeg);//seqUpDown(false,idOperation,idLeg);
break;
case 4:// 04 - Tourne Gauche //
+ //delaisNeed = (table_seqTurn_PAUSE_TIME[m_idFrame-1][0])*10;
+ //debug("\n\r asd:%i",delaisNeed);
seqTurn(false,m_idFrame,idLeg);
break;
case 5:// 05 - Tourne Droite //
+ //delaisNeed = (table_seqTurn_PAUSE_TIME[m_idFrame-1][0])*10;
+ //debug("\n\r seqt:%i",delaisNeed);
seqTurn(true,m_idFrame,idLeg);
break;
case 6:// 06 - Marche //
@@ -571,14 +643,22 @@
case 10:// 10 - Crabe Droite //
seqCrabe(true,m_idFrame,idLeg);
break;
+ case 11:// 11 - Position naturel //
+ defaultPosition(idLeg);
+ break;
}
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r delaisNeed: %i",delaisNeed);
+ debug(DEBUG_HOMEMADE_GETFRAME,"\n\r timeSequence: ");
+ for(int i=0; i<8; i++)debug(DEBUG_HOMEMADE_GETFRAME," [ %i ] ",timeSequence[i]);
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("\n\r seqturn:%i",delaisNeed);
debug(DEBUG_HOMEMADE_GETFRAME,"\n\r Out Get Frame");
return m_posLeg;
}
void homemadeSequence::seqUpDown(bool downUP,char idOperation,char idLeg)
{
+ delaisNeed = DELAITESTE;//(table_seqRepositionne_PAUSE_TIME[idOperation-1][0])*10;
if((idOperation<10)&&(idOperation>0)) {
switch(idLeg) { //moving hips
case 1://#0
@@ -742,9 +822,11 @@
}
void homemadeSequence::seqTurn(bool leftRIGHT,char idOperation,char idLeg)
{
- if(idOperation<32) {
+ if(idOperation<=SIZE_SEQTURN) {
+ m_posLeg[0]= 255;
for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqTurn[idOperation-1][idLeg-1][i-1];
delaisNeed = (table_seqTurn_PAUSE_TIME[idOperation-1][0])*10;
+ //debug("\n\r seqturn:%i",delaisNeed);
timeSequence[idLeg-1] = table_seqTurn_PAUSE_TIME[idOperation-1][1];
switch(idOperation) {
@@ -764,7 +846,7 @@
case 1://#0
m_posLeg[0]= (leftRIGHT)? 150:110;
break;
- case 7://28
+ case 8://28
m_posLeg[0]= (leftRIGHT)? 164:104;
break;
}
@@ -806,9 +888,9 @@
break;
//case :
//break;
- default:
- m_posLeg[0]= 255;
- break;
+ //default:
+ //m_posLeg[0]= 255;
+ // break;
}
@@ -819,100 +901,251 @@
void homemadeSequence::seqWalk(bool backFRONT,char idOperation,char idLeg)
{
if(backFRONT) {
- if(idOperation<41) {
+ if(idOperation <= SIZE_SEQFOWARD) {
+ m_posLeg[0]= 255;
for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqFoward[idOperation-1][idLeg-1][i-1];
delaisNeed = (table_seqFoward_PAUSE_TIME[idOperation-1][0])*10;
timeSequence[idLeg-1] = table_seqFoward_PAUSE_TIME[idOperation-1][1];
//manque qqch ici
- switch(idOperation)
- {
+ switch(idOperation) {
case 3:
- if(idLeg == 1) m_posLeg[0] = 150;
- else if(idLeg == 6) m_posLeg[0] = 123;
- break;
-
+ if(idLeg == 1) m_posLeg[0] = 150;
+ else if(idLeg == 6) m_posLeg[0] = 123;
+ break;
+
case 5:
- if((idLeg ==1) || (idLeg==6))
- timeSequence[idLeg-1] = 30;
- break;
-
+ if((idLeg ==1) || (idLeg==6))
+ timeSequence[idLeg-1] = 30;
+ break;
+
case 7:
- if(idLeg == 3)m_posLeg[0] = 187;
- else if(idLeg == 8) m_posLeg[0] = 123;
- break;
-
+ if(idLeg == 3)m_posLeg[0] = 187;
+ else if(idLeg == 8) m_posLeg[0] = 123;
+ break;
+
case 11:
- switch(idLeg)
- {
- case 1:
- m_posLeg[0] = 130;
+ switch(idLeg) {
+ case 1:
+ m_posLeg[0] = 130;
+ break;
+ case 2:
+ m_posLeg[0] = 140;
+ break;
+ case 3:
+ m_posLeg[0] = 160;
+ break;
+ case 4:
+ m_posLeg[0] = 117;
+ break;
+ case 5:
+ m_posLeg[0] = 179;
+ break;
+ case 6:
+ m_posLeg[0] = 134;
+ break;
+ case 7:
+ m_posLeg[0] = 183;
+ break;
+ case 8:
+ m_posLeg[0] = 144;
+ break;
+ }
break;
- case 2:
- m_posLeg[0] = 140;
- break;
- case 3:
- m_posLeg[0] = 160;
- break;
- case 4:
- m_posLeg[0] = 117;
+
+ case 16:
+ if(idLeg == 1) m_posLeg[0] = 137;
break;
- case 5:
- m_posLeg[0] = 179;
- break;
- case 6:
- m_posLeg[0] = 134;
+
+ case 20:
+ if(idLeg == 2) m_posLeg[0] = 176;
+ else if(idLeg == 7) m_posLeg[0] = 160;
break;
- case 7:
- m_posLeg[0] = 183;
+
+ case 22:
+ if((idLeg ==2) || (idLeg==7))
+ timeSequence[idLeg-1] = 30;
break;
- case 8:
- m_posLeg[0] = 144;
+
+ case 24:
+ if(idLeg == 4) m_posLeg[0] = 174;
+ else if(idLeg == 5) m_posLeg[0] = 133;
break;
- }
- break;
-
- case 16:
- if(idLeg == 1) m_posLeg[0] = 137;
- break;
-
- case 20:
- if(idLeg == 5) m_posLeg[0] = 176;
- else if(idLeg == 7) m_posLeg[0] = 160;
- break;
-
- case 22:
- if((idLeg ==2) || (idLeg==7))
- timeSequence[idLeg-1] = 30;
- break;
-
- case 24:
- if(idLeg == 4) m_posLeg[0] = 177;
- else if(idLeg == 5) m_posLeg[0] = 133;
- break;
-
+
+ case 27:
+ switch(idLeg) {
+ case 1:
+ m_posLeg[0] = 110;
+ break;
+ case 2:
+ m_posLeg[0] = 160;
+ break;
+ case 3:
+ m_posLeg[0] = 140;
+ break;
+ case 4:
+ m_posLeg[0] = 150;
+ break;
+ case 5:
+ m_posLeg[0] = 153;
+ break;
+ case 6:
+ m_posLeg[0] = 150;
+ break;
+ case 7:
+ m_posLeg[0] = 173;
+ break;
+ case 8:
+ m_posLeg[0] = 170;
+ break;
+ }
+ break;
+
+ case 34:
+ if(idLeg == 1) m_posLeg[0] = 130;
+ break;
+
+ case 38:
+ if(idLeg == 5) m_posLeg[0] = 153;//165;
+ break;
+
+ case 42:
+ if(idLeg == 2) m_posLeg[0] = 165;
+ break;
}
} else
for(int i=0; i <= 3; i++) m_posLeg[i]=0;
} else {
- if(idOperation<33) {
+ if(idOperation <= SIZE_SEQBACKWARD) {
+ m_posLeg[0]= 255;
for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqBackward[idOperation-1][idLeg-1][i-1];
delaisNeed = (table_seqBackward_PAUSE_TIME[idOperation-1][0])*10;
timeSequence[idLeg-1] = table_seqBackward_PAUSE_TIME[idOperation-1][1];
//manque qqch ici
+ switch(idOperation) {
+ case 3:
+ if(idLeg == 3) m_posLeg[0] = 140;
+ else if(idLeg == 8) m_posLeg[0] = 170;
+ break;
+
+ case 5:
+ if((idLeg ==3) || (idLeg==8))
+ timeSequence[idLeg-1] = 30;
+ break;
+
+ case 7:
+ if(idLeg == 1)m_posLeg[0] = 110;
+ else if(idLeg == 6) m_posLeg[0] = 150;
+ break;
+
+ case 11:
+ switch(idLeg) {
+ case 1:
+ m_posLeg[0] = 130;
+ break;
+ case 2:
+ m_posLeg[0] = 176;
+ break;
+ case 3:
+ m_posLeg[0] = 160;
+ break;
+ case 4:
+ m_posLeg[0] = 174;
+ break;
+ case 5:
+ m_posLeg[0] = 133;
+ break;
+ case 6:
+ m_posLeg[0] = 134;
+ break;
+ case 7:
+ m_posLeg[0] = 160;
+ break;
+ case 8:
+ m_posLeg[0] = 144;
+ break;
+ }
+ break;
+
+ case 13:
+ if(idLeg == 8) m_posLeg[0] = 144;
+ break;
+
+ case 17:
+ if(idLeg == 6) m_posLeg[0] = 133;
+ break;
+
+ case 22:
+ if(idLeg == 4) m_posLeg[0] = 117;
+ else if(idLeg == 5) m_posLeg[0] = 179;
+ break;
+
+ case 24:
+ if((idLeg ==4) || (idLeg==5))
+ timeSequence[idLeg-1] = 45;
+ break;
+
+ case 26:
+ if(idLeg == 2) m_posLeg[0] = 140;
+ else if(idLeg == 7) m_posLeg[0] = 190;
+ break;
+
+ case 29:
+ switch(idLeg) {
+ case 1:
+ m_posLeg[0] = 150;
+ break;
+ case 2:
+ m_posLeg[0] = 160;
+ break;
+ case 3:
+ m_posLeg[0] = 187;
+ break;
+ case 4:
+ m_posLeg[0] = 150;
+ break;
+ case 5:
+ m_posLeg[0] = 153;
+ break;
+ case 6:
+ m_posLeg[0] = 123;
+ break;
+ case 7:
+ m_posLeg[0] = 173;
+ break;
+ case 8:
+ m_posLeg[0] = 123;
+ break;
+ }
+ break;
+
+ case 33:
+ if(idLeg == 4) m_posLeg[0] = 150;
+ break;
+
+ /*case 38:
+ if(idLeg == 5) m_posLeg[0] = 165;
+ break;
+
+ case 42:
+ if(idLeg == 1) m_posLeg[0] = 165;
+ break;*/
+ }
} else
for(int i=0; i <= 3; i++) m_posLeg[i]=0;
}
}
void homemadeSequence::seqRepositioner(char idOperation,char idLeg)
{
- if(idOperation<6) {
+ if(idOperation <= SIZE_SEQREPOSITIONNE) {
+ m_posLeg[0]= 255;
+ delaisNeed = (table_seqRepositionne_PAUSE_TIME[idOperation-1][0])*10;
if(m_idLegUse == idLeg) {
for(int i=1; i <= 3; i++) m_posLeg[i]=table_seqRepositionne[idOperation-1][idLeg-1][i-1];
- timeSequence[idLeg-1] = table_seqTurn_PAUSE_TIME[idOperation-1][1];
+ timeSequence[idLeg-1] = table_seqRepositionne_PAUSE_TIME[idOperation-1][1];
if(idOperation == 5) {
switch(idLeg) {
@@ -944,16 +1177,19 @@
}
} else
- for(int i=0; i <= 3; i++)m_posLeg[i]=255;
- delaisNeed = (table_seqRepositionne_PAUSE_TIME[idOperation-1][0])*10;
+ for(int i=0; i < 4; i++)m_posLeg[i]=255;
+
} else
- for(int i=0; i <= 3; i++) m_posLeg[i]=0;
+ for(int i=0; i < 4; i++) m_posLeg[i]=0;
}
void homemadeSequence::seqCrabe(bool leftRIGHT,char idOperation,char idLeg)
{
- if(idOperation<33) {
+ if(idOperation<=SIZE_SEQCRABE) {
+ m_posLeg[0]= 255;
+ delaisNeed = 300;//DELAITESTE;//(table_seqRepositionne_PAUSE_TIME[idOperation-1][0])*10;
+ timeSequence[idLeg-1] = 30;
if(leftRIGHT)
- idOperation=33-idOperation;
+ idOperation=SIZE_SEQCRABE-idOperation;
switch(idLeg) { //moving hips
case 1:
@@ -985,41 +1221,55 @@
}
+void homemadeSequence::defaultPosition(char idLeg)
+{
+ if(m_idFrame == 1) {
+ timeSequence[idLeg-1] = 50;
+ for(int i=0;i<4;i++) m_posLeg[i]=table_seqDefaultPos[idLeg-1][i];
+ }
+ else
+ for(int i=0;i<4;i++) m_posLeg[i]= 0;
+}
bool homemadeSequence::next_frame(void)
{
debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r in Next Frame");
- bool temp=false;
+ bool result=false;
debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r m_idframe == %d",m_idFrame);
-
+ int tmpDelais=delaisNeed;
//if(m_idFrame > 1){
m_idFrame++;
unsigned char* tmpTable = get_frame(1);
+ debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r Leg1:");
+ for(int i=0; i<4; i++)debug(DEBUG_HOMEMADE_NEXTFRAME,"[%i]",tmpTable[i]);
+
if((tmpTable[0]!=0) && (tmpTable[1]!=0) && (tmpTable[2]!=0) && (tmpTable[3]!=0)) {
- temp=true;
+ result=true;
//m_idFrame++;//get_frame(m_idFrame);
- if((delaisNextSeq.read_ms() >= delaisNeed) || (delaisNeed == -1)) {
+ /*if((delaisNextSeq.read_ms() >= delaisNeed) || (delaisNeed == -1)) {
delaisNextSeq.reset();
- delaisNeed=-1;
- for(int i=0; i<8; i++)timeSequence[i]=0;
- } else
- m_idFrame--;
+ delaisNeed=-1;*/
+ for(int i=0; i<8; i++)timeSequence[i]=255;
+ //} else
+ // m_idFrame--;
} else
m_idFrame--;
//}
//}
+ delaisNeed=tmpDelais;
+
debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r m_idframe == %d",m_idFrame);
debug(DEBUG_HOMEMADE_NEXTFRAME,"\n\r out Next Frame");
- return temp;
+ return result;
}
bool homemadeSequence::prev_frame(void) //true == prev is ok
{
//false == impossible
debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r in Prev Frame");
- bool temp=false;
+ bool result=false;
debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r m_idframe == %d",m_idFrame);
@@ -1028,14 +1278,14 @@
unsigned char* tmpTable = get_frame(1);
if((tmpTable[0]!=0) && (tmpTable[1]!=0) && (tmpTable[2]!=0) && (tmpTable[3]!=0)) {
- temp=true;
+ result=true;
//m_idFrame++;//get_frame(m_idFrame);
- if((delaisNextSeq.read_ms() >= delaisNeed) || (delaisNeed == -1)) {
- delaisNextSeq.reset();
- delaisNeed=-1;
- for(int i=0; i<8; i++)timeSequence[i]=0;
- } else
- m_idFrame++;
+ /*if((delaisNextSeq.read_ms() >= delaisNeed) || (delaisNeed == -1)) {
+ delaisNextSeq.reset();*/
+ delaisNeed=-1;
+ for(int i=0; i<8; i++)timeSequence[i]=255;
+ //} else
+ // m_idFrame++;
} else
m_idFrame++;
@@ -1044,5 +1294,5 @@
debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r m_idframe == %d",m_idFrame);
debug(DEBUG_HOMEMADE_PREVFRAME,"\n\r out Prev Frame");
- return temp;
+ return result;
}
\ No newline at end of file
--- a/source/Motrice/homemadeSequence.h Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/homemadeSequence.h Thu May 14 21:40:23 2015 +0000
@@ -8,7 +8,8 @@
#ifndef HOMEMADESEQUENCE_H_
#define HOMEMADESEQUENCE_H_
-#define DEBUG_HOMEMADE_GETFRAME 1
+#define DEBUG_HOMEMADE_GETFRAME 0
+#define DEBUG_HOMEMADE_GETTIME 0
#define DEBUG_HOMEMADE_NEXTFRAME 0
#define DEBUG_HOMEMADE_PREVFRAME 0
#define DELAITESTE 1000
@@ -27,15 +28,15 @@
//X05 - Tourne Droite //
// 06 - Marche //
// 07 - Recule //
- // 08 - Repositioner //
- // 09 - Crabe Gauche //
- // 10 - Crabe Droite //
- // 11 - recalibration legs //
+ // 08 - Repositioner legs //
+ // 09 - Crabe Gauche // Dont use Crabe because it's really anoying
+ // 10 - Crabe Droite // Dont use Crabe because it's really anoying
+ // 11 - Position naturel //
//////////////////////////////
char m_SequenceChosen;
char m_idFrame;
- Timer delaisNextSeq;
+ //Timer delaisNextSeq;//move to movement.h
int delaisNeed;
unsigned char m_posLeg[4];
unsigned char timeSequence[8];
@@ -46,24 +47,30 @@
void seqWalk(bool backFRONT,char idOperation,char idLeg);
void seqRepositioner(char idOperation,char idLeg);
void seqCrabe(bool leftRIGHT,char idOperation,char idLeg);
-
+ void defaultPosition(char idLeg);
+
+ int patchDelais(char idMode);
+
public:
homemadeSequence();
~homemadeSequence();
void set_Sequence(char idSequence) {
m_SequenceChosen = idSequence;
m_idFrame = 1;
- delaisNextSeq.reset();
- delaisNeed=-1;
+
+ //delaisNeed=patchDelais(idSequence);
}
char get_Sequence(void) {
return m_SequenceChosen;
}
void set_leg(char idLeg){m_idLegUse = idLeg;}
- unsigned char get_Time(char idLeg){
- unsigned char result = timeSequence[idLeg-1];
- return (result==255)? DELAITESTE:result;
+ int get_msTime(char idLeg){
+ int result = timeSequence[idLeg-1];
+
+ debug(DEBUG_HOMEMADE_GETTIME,"\n\r idLeg:%i \n\r timeSequence:%i\n\r delais:%i",idLeg,result,delaisNeed);
+ return (result==255)? DELAITESTE/2:result*10;
}
+ int get_delaisNeed_Ms(){return delaisNeed;}
unsigned char* get_frame(/*char idSequence,*/char idOperation,char idLeg);
unsigned char* get_frame(/*char idSequence,char idOperation,*/char idLeg);
void set_frameID(char position) {m_idFrame = position;};
--- a/source/Motrice/mouvement.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/mouvement.cpp Thu May 14 21:40:23 2015 +0000
@@ -11,15 +11,16 @@
Faculter_motrice::Faculter_motrice(Serial* com):m_com(com)
{
m_seq = new homemadeSequence();
+ delaisNextSeq.reset();
m_arr_D = new Patte(1,0,1,2,3);
m_arr_mil_D = new Patte(2,4,5,6,7);
- m_arr_G = new Patte(3,16,17,18,19);
- m_arr_mil_G = new Patte(4,20,21,22,23);
- m_avv_D = new Patte(5,12,13,14,15);
- m_avv_mil_D = new Patte(6,8,9,10,11);
- m_avv_G = new Patte(7,28,29,30,31);
- m_avv_mil_G = new Patte(8,24,25,26,27);
+ m_arr_G = new Patte(5,16,17,18,19);
+ m_arr_mil_G = new Patte(6,20,21,22,23);
+ m_avv_D = new Patte(4,12,13,14,15);
+ m_avv_mil_D = new Patte(3,8,9,10,11);
+ m_avv_G = new Patte(8,28,29,30,31);
+ m_avv_mil_G = new Patte(7,24,25,26,27);
m_SequenceID_arr_D =0;
m_SequenceID_arr_mil_D =0;
@@ -32,6 +33,8 @@
m_ForceStop = false;
m_CriticalStop = false;
+
+
}
Faculter_motrice::~Faculter_motrice()
{
@@ -59,69 +62,81 @@
void Faculter_motrice::exec(void)
{
debug(DEBUG_MOUVEMENT,"\n\r In Faculter Mott");
- debug(DEBUG_MOUVEMENT,"\n\r Flag in exec [%i :: %i ] \n\r",m_ForceStop,m_seq->get_Sequence());
+ debug(DEBUG_MOUVEMENT,"\n\r m_ForceStop : %i\n\r Sequence: %i\n\r",m_ForceStop,m_seq->get_Sequence());
if((!m_ForceStop)&&(m_seq->get_Sequence() != 0)) {
- unsigned char* tempLeg;
+ if((delaisNextSeq.read_ms() >= m_seq->get_delaisNeed_Ms()) || (m_seq->get_delaisNeed_Ms() == -1)) {
+
+ delaisNextSeq.stop();
+ debug(DEBUG_MOUVEMENT,"\n\rdelais:%i / mesure:%i",m_seq->get_delaisNeed_Ms(),delaisNextSeq.read_ms());
+ unsigned char* tempLeg;
- //switch(m_seq->get_Sequence()) {
- // default:
- tempLeg=m_seq->get_frame(m_arr_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_mil_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_arr_mil_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_mil_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_mil_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_D->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- tempLeg=m_seq->get_frame(m_avv_G->id());
- if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
- m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
-
- // //...//
- if(m_seq->next_frame() == false) {
- m_ForceStop=true;
+ //switch(m_seq->get_Sequence()) {
+ // default:
+ tempLeg=m_seq->get_frame(m_arr_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_mil_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_arr_mil_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_arr_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_mil_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_mil_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_mil_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_mil_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
- debug(DEBUG_EXEC," Flag next impossible \n\r");
+ tempLeg=m_seq->get_frame(m_avv_D->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_D->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ tempLeg=m_seq->get_frame(m_avv_G->id());
+ //if((tempLeg[0]!=255) && (tempLeg[1]!=255) && (tempLeg[2]!=255) && (tempLeg[3]!=255))
+ m_avv_G->move(tempLeg[0],tempLeg[1],tempLeg[2],tempLeg[3]);
+
+ // //...//
+
+
+ sendSequence();
- }
- else
- debug(DEBUG_EXEC," Flag next possible \n\r");
+ delaisNextSeq.reset();
+ bool gga =m_seq->next_frame();
+ if(/*m_seq->next_frame()*/gga == false) {
+ m_ForceStop=true;
+
+ debug(DEBUG_EXEC," Flag next impossible \n\r");
+
+ } else {
+ debug(DEBUG_EXEC," Flag next possible \n\r");
+ delaisNextSeq.start();
+ }
+ }
// break;
//}
//char buffer[50];
//scanf(buffer,"%s T%d\n\r",m_arr_D->toString(),DELAITESTE);
//m_com->printf("%s T%d\n\r",m_arr_D->toString(),DELAITESTE); //move to sendSequence()
-
+
//debug(DEBUG_MOUVEMENT,"\n\r %s T%d\n\r",m_arr_D->toString(),);
- sendSequence();
+
/*
tempLeg=m_seq->get_frame(m_arr_D->id());
m_com->printf("This program is %d .\n", tempLeg[0]);*/
}
-debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
+ debug(DEBUG_MOUVEMENT,"\n\r Out Faculter Mott");
}
/*void Faculter_motrice::moveLeft (void)
{
@@ -155,6 +170,7 @@
{
}*/
+
void Faculter_motrice::crit_stop(void)
{
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");
@@ -164,30 +180,39 @@
void Faculter_motrice::sendSequence(void)
{
- debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
- m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_D->toString(),m_seq->get_Time(m_arr_D->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_G->toString(),m_seq->get_Time(m_arr_G->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_D->toString(),m_seq->get_Time(m_arr_mil_D->id()));
-
- m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_arr_mil_G->toString(),m_seq->get_Time(m_arr_mil_G->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_D->toString(),m_seq->get_Time(m_avv_mil_D->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_mil_G->toString(),m_seq->get_Time(m_avv_mil_G->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_D->toString(),m_seq->get_Time(m_avv_D->id()));
-
- m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
- debug(DEBUG_SENDSEQ,"\n\r %s T%d",m_avv_G->toString(),m_seq->get_Time(m_avv_G->id()));
-
- debug(DEBUG_SENDSEQ,"\n\r\n\r out sendSeq");
+ debug(DEBUG_SENDSEQ,"\n\r in sendSeq\n\r");
+ debug(DEBUG_SENDSEQ,"\n\r tarzan: %i",m_seq->get_msTime(m_arr_mil_D->id()));
+ if(m_arr_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_D->toString(),m_seq->get_msTime(m_arr_D->id()));
+ }
+ if(m_arr_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_G->toString(),m_seq->get_msTime(m_arr_G->id()));
+ }
+ if(m_arr_mil_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_D->toString(),m_seq->get_msTime(m_arr_mil_D->id()));
+ }
+ if(m_arr_mil_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_arr_mil_G->toString(),m_seq->get_msTime(m_arr_mil_G->id()));
+ }
+ if(m_avv_mil_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_D->toString(),m_seq->get_msTime(m_avv_mil_D->id()));
+ }
+ if(m_avv_mil_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_mil_G->toString(),m_seq->get_msTime(m_avv_mil_G->id()));
+ }
+ if(m_avv_D->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_D->toString(),m_seq->get_msTime(m_avv_D->id()));
+ }
+ if(m_avv_G->haveChange()) {
+ m_com->printf("%s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
+ debug(DEBUG_SENDSEQ,"\n\r %s T%d\n\r",m_avv_G->toString(),m_seq->get_msTime(m_avv_G->id()));
+ }
+ debug(DEBUG_SENDSEQ,"\n\r out sendSeq\n\r");
}
\ No newline at end of file
--- a/source/Motrice/mouvement.h Mon May 11 13:48:38 2015 +0000
+++ b/source/Motrice/mouvement.h Thu May 14 21:40:23 2015 +0000
@@ -24,9 +24,9 @@
#define MOUVEMENT_H_
-#define DEBUG_MOUVEMENT 1
-#define DEBUG_EXEC 1
-#define DEBUG_SENDSEQ 1
+#define DEBUG_MOUVEMENT 0/*1*/
+#define DEBUG_EXEC 0/*1*/
+#define DEBUG_SENDSEQ 0/*1*/
#include "mbed.h"
//#include <string>
@@ -40,6 +40,7 @@
{
Serial* m_com;
homemadeSequence* m_seq;
+ Timer delaisNextSeq;
Patte *m_arr_D;
char m_SequenceID_arr_D;
@@ -57,7 +58,7 @@
char m_SequenceID_avv_G;
Patte *m_avv_mil_G;
char m_SequenceID_avv_mil_G;
-
+
// Flag //
bool m_ForceStop; // use when you want pause the motion
bool m_CriticalStop; // use to turn off all legs
@@ -75,7 +76,7 @@
Faculter_motrice(Serial* com);
virtual ~Faculter_motrice();
-
+ bool isSeqComplet(void){return m_ForceStop;}
void exec(void);
void stop(void) {
m_ForceStop= true;
@@ -83,47 +84,61 @@
void resume(void) {
m_ForceStop=false;
m_CriticalStop=false;
+ delaisNextSeq.start();
}
void crit_stop(void);
void moveLeft (void) {
m_ForceStop=false;
m_seq->set_Sequence(9);
+ delaisNextSeq.start();
}
void moveRight(void){
m_ForceStop=false;
m_seq->set_Sequence(10);
+ delaisNextSeq.start();
}
void moveUp (void){
m_ForceStop=false;
m_seq->set_Sequence(2);
+ delaisNextSeq.start();
}
void moveDown (void){
m_ForceStop=false;
m_seq->set_Sequence(3);
+ delaisNextSeq.start();
}
void moveFront(void){
m_ForceStop=false;
m_seq->set_Sequence(6);
+ delaisNextSeq.start();
}
void moveBack (void){
m_ForceStop=false;
m_seq->set_Sequence(7);
+ delaisNextSeq.start();
}
void turnLeft (void){
m_ForceStop=false;
m_seq->set_Sequence(4);
+ delaisNextSeq.start();
}
void trunRight(void){
m_ForceStop=false;
m_seq->set_Sequence(5);
+ delaisNextSeq.start();
}
- void calibre (char idLeg){
+ void calibre (){
m_ForceStop=false;
- m_seq->set_Sequence(1);
+ m_seq->set_Sequence(11);
+ delaisNextSeq.start();
+ }
+ void repositioner(char idLeg){
+ m_ForceStop=false;
m_seq->set_leg(idLeg);
+ m_seq->set_Sequence(8);
+ delaisNextSeq.start();
}
- //void repositioner(char idLeg);
--- a/source/OSNAPprotocoleDefine.h Mon May 11 13:48:38 2015 +0000
+++ b/source/OSNAPprotocoleDefine.h Thu May 14 21:40:23 2015 +0000
@@ -37,6 +37,11 @@
}osnapRegisterD;
};
+typedef enum {
+ NON_DETERMINER = 0,
+ DIGITALE = 1,
+ ANALOGIQUE=2
+} TypeDeFormat;
typedef enum {
//NONE =0,
--- a/source/Task/TTaskCritique.cpp Mon May 11 13:48:38 2015 +0000
+++ b/source/Task/TTaskCritique.cpp Thu May 14 21:40:23 2015 +0000
@@ -7,7 +7,6 @@
m_CtrlBridge->initCom();
m_ListDesModules = m_CtrlBridge->findModule(0,0,0,0); //get all modules
-
tymy=true;
}
@@ -47,8 +46,9 @@
flag.append(1,0);//0x02);
for(int i=0; i<m_ListDesModules.length(); ++i)
{
+ wait_us(300);
flag.clear();
- flag.append(1,6);
+ flag.append(1,7);
data.clear();
debug("\n\r result: %d",m_CtrlBridge->send(m_ListDesModules.at(i),flag,data));
if(flag[0]== 0x02)