diff --git a/.gitignore b/.gitignore index be303c9..1331283 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,7 @@ *BASE* *REMOTE* *.swp + +FirmwareRope/\.vscode/ + +FirmwareRope/build/ diff --git a/FirmwareRope/FirmwareRope.ino b/FirmwareRope/FirmwareRope.ino index 65be0c7..cb5f416 100644 --- a/FirmwareRope/FirmwareRope.ino +++ b/FirmwareRope/FirmwareRope.ino @@ -1,14 +1,30 @@ +#include #include "libs/Button/Button.cpp" #include "libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp" + #define QUANTIDADE_MAXIMA_ACOES 45 + bool testing_loop = 0; bool sound_off = 0; + int easter_egg; int TURN_STEP_OVERWRITE = 170; int WALK_STEP_OVERWRITE = 360; -//Entradas +//Calibracao +#define FRENTE 0 +#define TRAS 1 +#define DIREITA 2 +#define ESQUERDA 3 + +bool calibrando = 0; +int steps[] = {0,0,0,0}; +int posicaoMem[] = {0,2,4,6}; +int valoresCalibracao[] = {100,1,50,10}; +int stepSelecionado = -1; + +//Entradas Button btnTras = Button (A1); Button btnDireita = Button (A2); Button btnIr = Button (A3); @@ -200,24 +216,67 @@ void verificarFeedback(int acoesContExec) } } +void salvarStep(int endereco, int valor) +{ + byte hiByte = highByte(valor); + byte loByte = lowByte(valor); + EEPROM.write(endereco, hiByte); + EEPROM.write(endereco+1, loByte); +} + +int lerStep(int endereco) +{ + byte hiByte = EEPROM.read(endereco); + byte loByte = EEPROM.read(endereco+1); + return word(hiByte, loByte); +} + +void limparMemoria() +{ + steps[FRENTE] = 0; + steps[TRAS] = 0; + steps[DIREITA] = 0; + steps[ESQUERDA] = 0; + salvarStep(posicaoMem[ESQUERDA], 0); + salvarStep(posicaoMem[DIREITA], 0); + salvarStep(posicaoMem[FRENTE], 0); + salvarStep(posicaoMem[TRAS], 0); +} + void verificarInstrucao(int acoesContExec) { switch (acoes[acoesContExec]) { case acaoEsquerda: - motores_esquerda(rope_foi_parado,TURN_STEP_OVERWRITE); + if(steps[ESQUERDA]>0){ + motores_esquerda(rope_foi_parado,steps[ESQUERDA]); + }else{ + motores_esquerda(rope_foi_parado,TURN_STEP_OVERWRITE); + } break; case acaoDireita: - motores_direita(rope_foi_parado,TURN_STEP_OVERWRITE); + if(steps[DIREITA]>0){ + motores_direita(rope_foi_parado,steps[DIREITA]); + }else{ + motores_direita(rope_foi_parado,TURN_STEP_OVERWRITE); + } break; case acaoFrente: - motores_frente(rope_foi_parado); + if(steps[FRENTE]>0){ + motores_frente(rope_foi_parado, steps[FRENTE]); + }else{ + motores_frente(rope_foi_parado); + } break; case acaoTras: - motores_tras(rope_foi_parado); + if(steps[TRAS]>0){ + motores_tras(rope_foi_parado, steps[TRAS]); + }else{ + motores_tras(rope_foi_parado); + } break; default: @@ -247,7 +306,10 @@ void executar() { easter_egg = -1; if (acoesContExec < acoesContProg) { - verificarFeedback(acoesContExec); + if(!testing_loop) + { + verificarFeedback(acoesContExec); + } verificarInstrucao(acoesContExec); } @@ -293,7 +355,63 @@ bool rope_foi_parado(){ btnIr.process(); return ESTADO_ATUAL == ESTADO_AGUARDANDO ? true : false; } + +void andarPassosCalibracao(int direcao) +{ + if(stepSelecionado == FRENTE) + { + ESTADO_ATUAL = ESTADO_EXECUTANDO; + motores_frente(rope_foi_parado, valoresCalibracao[direcao]); + steps[FRENTE]+=valoresCalibracao[direcao]; + ESTADO_ATUAL = ESTADO_AGUARDANDO; + } + else if(stepSelecionado == TRAS) + { + ESTADO_ATUAL = ESTADO_EXECUTANDO; + motores_tras(rope_foi_parado, valoresCalibracao[direcao]); + steps[TRAS]+=valoresCalibracao[direcao]; + ESTADO_ATUAL = ESTADO_AGUARDANDO; + } + else if(stepSelecionado == DIREITA) + { + ESTADO_ATUAL = ESTADO_EXECUTANDO; + motores_direita(rope_foi_parado, valoresCalibracao[direcao]); + steps[DIREITA]+=valoresCalibracao[direcao]; + ESTADO_ATUAL = ESTADO_AGUARDANDO; + } + else if(stepSelecionado == ESQUERDA) + { + ESTADO_ATUAL = ESTADO_EXECUTANDO; + motores_esquerda(rope_foi_parado, valoresCalibracao[direcao]); + steps[ESQUERDA]+=valoresCalibracao[direcao]; + ESTADO_ATUAL = ESTADO_AGUARDANDO; + } +} + void onEsquerdaPress(Button &b){ + if(calibrando && stepSelecionado < 0) + { + stepSelecionado = ESQUERDA; + steps[stepSelecionado] = 0; + feedbackEsquerda(true); + delay(100); + // ESTADO_ATUAL = ESTADO_EXECUTANDO; + // steps[ESQUERDA] = motores_esquerda_calibracao(rope_foi_parado); + // ESTADO_ATUAL = ESTADO_AGUARDANDO; + // feedbackEsquerda(true); + // delay(100); + // salvarStep(posicaoMem[ESQUERDA], steps[ESQUERDA]); + return; + } + if(calibrando && stepSelecionado >= 0) + { + feedbackEsquerda(true); + delay(100); + feedbackEsquerda(true); + delay(1000); + andarPassosCalibracao(ESQUERDA); + return; + } if (acoesContProg < QUANTIDADE_MAXIMA_ACOES) { acoes[acoesContProg] = acaoEsquerda; @@ -303,6 +421,29 @@ void onEsquerdaPress(Button &b){ } } void onDireitaPress(Button &b){ + if(calibrando && stepSelecionado < 0) + { + stepSelecionado = DIREITA; + steps[stepSelecionado] = 0; + feedbackDireita(true); + delay(100); + // ESTADO_ATUAL = ESTADO_EXECUTANDO; + // steps[DIREITA] = motores_direita_calibracao(rope_foi_parado); + // ESTADO_ATUAL = ESTADO_AGUARDANDO; + // feedbackDireita(true); + // delay(100); + // salvarStep(posicaoMem[DIREITA], steps[DIREITA]); + return; + } + if(calibrando && stepSelecionado >= 0) + { + feedbackDireita(true); + delay(100); + feedbackDireita(true); + delay(1000); + andarPassosCalibracao(DIREITA); + return; + } if (acoesContProg < QUANTIDADE_MAXIMA_ACOES) { acoes[acoesContProg] = acaoDireita; @@ -312,6 +453,29 @@ void onDireitaPress(Button &b){ } } void onFrentePress(Button &b){ + if(calibrando && stepSelecionado < 0) + { + stepSelecionado = FRENTE; + steps[stepSelecionado] = 0; + feedbackFrente(true); + delay(100); + // ESTADO_ATUAL = ESTADO_EXECUTANDO; + // steps[FRENTE] = motores_frente_calibracao(rope_foi_parado); + // ESTADO_ATUAL = ESTADO_AGUARDANDO; + // feedbackFrente(true); + // delay(100); + // salvarStep(posicaoMem[FRENTE], steps[FRENTE]); + return; + } + if(calibrando && stepSelecionado >= 0) + { + feedbackFrente(true); + delay(100); + feedbackFrente(true); + delay(1000); + andarPassosCalibracao(FRENTE); + return; + } if (acoesContProg < QUANTIDADE_MAXIMA_ACOES) { acoes[acoesContProg] = acaoFrente; @@ -321,6 +485,29 @@ void onFrentePress(Button &b){ } } void onTrasPress(Button &b){ + if(calibrando && stepSelecionado < 0) + { + stepSelecionado = TRAS; + steps[stepSelecionado] = 0; + feedbackTras(true); + delay(100); + // ESTADO_ATUAL = ESTADO_EXECUTANDO; + // steps[TRAS] = motores_tras_calibracao(rope_foi_parado); + // ESTADO_ATUAL = ESTADO_AGUARDANDO; + // feedbackTras(true); + // delay(100); + // salvarStep(posicaoMem[TRAS], steps[TRAS]); + return; + } + if(calibrando && stepSelecionado >= 0) + { + feedbackTras(true); + delay(100); + feedbackTras(true); + delay(1000); + andarPassosCalibracao(TRAS); + return; + } if (acoesContProg < QUANTIDADE_MAXIMA_ACOES) { acoes[acoesContProg] = acaoTras; @@ -330,6 +517,20 @@ void onTrasPress(Button &b){ } } void onIrPress(Button &b){ + if(calibrando && stepSelecionado >= 0) + { + feedbackFrente(true); + delay(50); + feedbackTras(true); + delay(50); + feedbackEsquerda(true); + delay(50); + feedbackDireita(true); + delay(50); + salvarStep(posicaoMem[stepSelecionado], steps[stepSelecionado]); + stepSelecionado=-1; + return; + } if(acoesContProg == 0 && easter_egg >= 0){ easter_egg++; } @@ -344,6 +545,33 @@ void onIrPress(Button &b){ delay(100); } +void onIrHold(Button &b){ + feedbackEasterEggActivated(); + + if(easter_egg > 5) + { + limparMemoria(); + feedbackFrente(true); + delay(50); + feedbackTras(true); + delay(50); + feedbackEsquerda(true); + delay(50); + feedbackDireita(true); + delay(50); + return; + } + acoesContProg = 0; + if(calibrando) + { + calibrando = 0; + } + else{ + calibrando = 1; + } + delay(100); +} + void definirCallBack() { btnTras.pressHandler(onTrasPress); @@ -351,6 +579,7 @@ void definirCallBack() btnEsquerda.pressHandler(onEsquerdaPress); btnDireita.pressHandler(onDireitaPress); btnIr.pressHandler(onIrPress); + btnIr.holdHandler(onIrHold, 5000); } void setup_processar_estados_invalidos_iniciacao(){ @@ -362,6 +591,13 @@ void setup_processar_estados_invalidos_iniciacao(){ btnIr.process(); } +void load_steps(){ + steps[DIREITA] = lerStep(posicaoMem[DIREITA]); + steps[ESQUERDA] = lerStep(posicaoMem[ESQUERDA]); + steps[FRENTE] = lerStep(posicaoMem[FRENTE]); + steps[TRAS] = lerStep(posicaoMem[TRAS]); +} + void setup() { Serial.begin(9600); @@ -373,12 +609,13 @@ void setup() { pinMode(SAIDA_SOM, OUTPUT); steppers_setup(); - definirCallBack(); + load_steps(); + definirCallBack(); ESTADO_ATUAL = ESTADO_AGUARDANDO; - - setup_processar_estados_invalidos_iniciacao(); easter_egg = 0; + + setup_processar_estados_invalidos_iniciacao(); } void loop() @@ -397,4 +634,4 @@ void loop() break; } //instrucoes_de_teste(); -} +} diff --git a/FirmwareRope/libs/Button/Button.cpp b/FirmwareRope/libs/Button/Button.cpp index 1d96d01..f75b506 100644 --- a/FirmwareRope/libs/Button/Button.cpp +++ b/FirmwareRope/libs/Button/Button.cpp @@ -103,12 +103,13 @@ void Button::process(void) if (bitRead(state,CURRENT) != bitRead(state,PREVIOUS)) { unsigned int interval = millis() - debounceStartTime; - // if(debounceMode){ - // Serial.print("debounceStartTime: "); - // Serial.print(debounceStartTime); - // Serial.print("\tdebounceDuration: "); - // Serial.println(debounceDuration); - // Serial.println(interval); + //Serial.print("State Changed"); + // if(debounceMode){ + // Serial.print("debounceStartTime: "); + // Serial.print(debounceStartTime); + // Serial.print("\tdebounceDuration: "); + // Serial.println(debounceDuration); + // Serial.println(interval); // } if(debounceMode && interval < debounceDuration){ // not enough time has passed; ignore @@ -117,7 +118,7 @@ void Button::process(void) // Serial.println("state changed"); debounceStartTime = millis(); //the state changed to PRESSED - if (bitRead(state,CURRENT) == true) + if (bitRead(state,CURRENT) == false) { numberOfPresses++; if (cb_onPress) { cb_onPress(*this); } //fire the onPress event @@ -136,6 +137,7 @@ void Button::process(void) } else { + //Serial.print("State did not Changed"); //note that the state did not change bitWrite(state,CHANGED,false); //should we trigger an onHold event? diff --git a/FirmwareRope/libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp b/FirmwareRope/libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp index f0dd33e..f62f121 100644 --- a/FirmwareRope/libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp +++ b/FirmwareRope/libs/RoPE_Steppers_28BYJ48/RoPE_Steppers_28BYJ48.cpp @@ -175,6 +175,87 @@ void motores_direita(bool(*callback)(), int steps = (((360*DISTANCIA_GIRO)/(DIA } } +//Calibração +int motores_frente_calibracao(bool(*callback)()){ + int steps = 0; + while(!((*callback)()) ){ + private_RoPE_Steppers_28BYJ48_MOTOR1_step_one(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_one(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_two(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_four(); + delay(DELAY_MAX-1); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_three(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_three(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_four(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_two(); + delay(DELAY_MAX-1); + steps++; + } + return steps; +} + +int motores_tras_calibracao(bool(*callback)()){ + int steps = 0; + while(!((*callback)()) ){ + private_RoPE_Steppers_28BYJ48_MOTOR1_step_one(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_one(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_four(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_two(); + delay(DELAY_MAX-1); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_three(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_three(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_two(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_four(); + delay(DELAY_MAX-1); + steps++; + } + return steps; +} + +int motores_esquerda_calibracao(bool(*callback)()){ + int steps = 0; + while(!((*callback)()) ){ + private_RoPE_Steppers_28BYJ48_MOTOR1_step_one(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_one(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_two(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_two(); + delay(DELAY_MAX-1); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_three(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_three(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_four(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_four(); + delay(DELAY_MAX-1); + steps++; + } + return steps; +} + +int motores_direita_calibracao(bool(*callback)()){ + int steps = 0; + while(!((*callback)()) ){ + private_RoPE_Steppers_28BYJ48_MOTOR1_step_one(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_one(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_four(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_four(); + delay(DELAY_MAX-1); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_three(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_three(); + delay(DELAY_MAX); + private_RoPE_Steppers_28BYJ48_MOTOR1_step_two(); + private_RoPE_Steppers_28BYJ48_MOTOR2_step_two(); + delay(DELAY_MAX-1); + steps++; + } + return steps; +} + void desligar_motores(){ private_RoPE_Steppers_28BYJ48_MOTORS_LOW(); } \ No newline at end of file