/* R2D2
* Control robot R2D2 que tenia el Victor
*
* Comandes via interruptor del mig son sequencials
* Comandes via Blueetooth
*
* Versio ESP32
*
* 5/3/20 - algun canvi menor pero sense provar!!! <-----
* 2/3/20 - motor braços funcionant
* 22/2/20 - motor cap
* 9/2/20 - falla el dfplayer
*
*/
/*
* NOM BT - DEPEN DEL XIP!
*/
#define NOM_BT "R2D2_ESP32"
//#define NOM_BT "R2D2_ESP32_2"
/*
* CONFIG PROVES
*/
#define VERSIO 20200308
int debug_on = 1;
#define INICIALITZAR_DFPLAYER 1 // ho trec de vegades mentre no arreglo el soroll
#define INICIALITZAR_BT 1
#define BOTO_PIT_CONNECTAT 0 // quan faig proves sense connectar sensors cal posar aixo a zero
// si no detecta soroll i es pensa que estic activant sequencia d'accions
#define MAX_FOLDERS_SD 3
#include "BluetoothSerial.h" //Header File for Serial Bluetooth, will be added by default into Arduino
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#include "DFRobotDFPlayerMini.h" // provat feb20 amb 1.0.3
const int pinCapDret = 32;
const int pinCapMig = 35;
const int pinCapEsq = 34;
const int pinIntPit = 5;
const int pinPeu = 33;
const int pinLedBlau = 21;
const int pinLedVermell = 19;
const int pinRX2 = 16;
const int pinTX2 = 17;
const int pinMotorBracEsqPWM = 22; // 13; // A1 - pwm
const int pinMotorBracEsqDIR = 23; // 12; // A2 - direccio
const int pinMotorCapPWM = 14; // B1 - pwm
const int pinMotorCapDIR = 27; // B2 - direccio - forward ha de ser cap a l'esquerra
const int pinMotorBracDretPWM = 26; // D1 - pwm
const int pinMotorBracDretDIR = 25; // D2 - direccio
const int pwmChannelBracEsq = 2;
const int pwmChannelBracDret = 1;
const int pwmChannelCap = 0;
/*
* instructions greater than 200 expect a parameter
* they have a _PARAM sufix
*
*/
#define LED_VERMELL 1 // light red led TBD miliseconds
#define LED_VERMELL_PARAM 201 // light red led times
#define LED_BLAU 2 // light blue led TBD miliseconds
#define LED_BLAU_PARAM 202 // light blue led times
#define LED_TOTS 3 // ???
#define LED_VERMELL_1_SEG 4
#define LED_BLAU_1_SEG 5
#define SOUND_PLAY_PARAM 210 // play track from root (folder 0)
#define EFFECTS_PLAY_PARAM 211 // play track from folder 1 <-- no funcionene
#define MUSICA_PLAY_PARAM 212 // play track from folder 2 <-- no funcionene
#define FRASE_PLAY_PARAM 213 // play track from folder 3 <-- no funcionene
#define SOUND_1 11 // from root
#define SOUND_2 12
#define SOUND_3 13
#define SOUND_4 14
#define SOUND_5 15
#define SOUND_6 16
#define SOUND_7 17
#define EFFECTS_NEXT 21 // play next sound, in folder "1" <---- no funciona
#define MUSICA_NEXT 22 // play next music, in folder "2" <---- no funciona
#define FRASE_NEXT 23 // play next frase, in folder "3" <---- no funciona
#define GIR_DRETA 30
#define GIR_ESQUERRA 31
#define MOTORS_PARAR 32
#define ENDAVANT 33 // go forward 2000 ms
#define ENDAVANT_PARAM 233 // go forward ms
#define ENDARRERA 34 // go backward 2000 ms
#define ENDARRERA_PARAM 234 // go backward ms
#define VELOCITAT_ESQ_PARAM 235 // set left arm speed to (0..255)
#define VELOCITAT_DRET_PARAM 236 // set right arm speed to (0..255)
#define CAP_ESQUERRA 40 // turn head left to end
#define CAP_ESQUERRA_PARAM 240 // turn head left miliseconds
#define CAP_ESQUERRA_POC 41 // turn head right 500 miliseconds
#define CAP_DRETA 42 // turn head right to end
#define CAP_DRETA_PARAM 242 // turn head right miliseconds
#define CAP_DRETA_POC 43 // turn head right 500 miliseconds
#define CAP_MIG 44 // turn head to look at front, start going left
#define VELOCITAT_CAP_PARAM 245 // set head speed to (0..255)
#define DEBUG_ON 90
#define DEBUG_OFF 91
#define VOLUM_PUJAR 92
#define VOLUM_BAIXAR 93
#define SHOW_STATUS 94
const int max_accions = 9; // numero d'accions, es fa servir per fer bucle accions amb boto pit TEMPORAL<--
int compt_accions = 1;
enum direccio {DRETA, ESQUERRA};
enum color_led {BLUE, RED};
// gira esquerra quan es fa forward i dreta quan es fa backward
int velocitat_gir_cap_esq = 30; // 30 va be a 4.5v. (entre 0 i 60)
int velocitat_gir_cap_dret = 30;
int velocitat_gir_brac_esq = 30;
int velocitat_gir_brac_dret = 30;
int sd_folder_files[MAX_FOLDERS_SD]; // numero fitxers en folder n de la tarja del dfplayer
int sd_sound_next[MAX_FOLDERS_SD]; // proxima canço a tocar si faig next
#define TEMPS_GIR 2000 // temps que estan els motors encesos quan giren TBC, milisegons
// 3 objectes comunicacio serial: consola, dfplayer i bluetooth
HardwareSerial mySoftwareSerial(1); // DFPlayer
BluetoothSerial BT; // BT
DFRobotDFPlayerMini myDFPlayer; // mp3 player
int dfplayer_inicialitzat = 0;
int bt_inicialitzat = 0;
int volum_dfplayer = 20; //Set volume value. From 0 to 30
int sound_folder_1_init = 0;
/*
* SETUP
*
*/
void setup() {
pinMode (pinMotorCapPWM, OUTPUT);
digitalWrite(pinMotorCapPWM,LOW);
pinMode (pinMotorCapDIR, OUTPUT);
digitalWrite(pinMotorCapDIR,LOW);
pinMode (pinMotorBracEsqPWM, OUTPUT);
digitalWrite(pinMotorBracEsqPWM,LOW);
pinMode (pinMotorBracEsqDIR, OUTPUT);
digitalWrite(pinMotorBracEsqDIR,LOW);
pinMode (pinMotorBracDretPWM, OUTPUT);
digitalWrite(pinMotorBracDretPWM,LOW);
pinMode (pinMotorBracDretDIR, OUTPUT);
digitalWrite(pinMotorBracDretDIR,LOW);
// pinMode (pinRX2, OUTPUT); no han d'estar a output!!!
// pinMode (pinTX2, OUTPUT);
pinMode (pinCapEsq, INPUT);
pinMode (pinCapMig, INPUT);
pinMode (pinCapDret, INPUT);
pinMode (pinIntPit, INPUT);
pinMode (pinPeu, INPUT);
pinMode (pinLedVermell, OUTPUT); // reset led
digitalWrite(pinLedVermell, HIGH);
pinMode (pinLedBlau, OUTPUT);
digitalWrite(pinLedBlau, HIGH); // reset led
pinMode (LED_BUILTIN, OUTPUT);
// Inicialitza consola
led(BLUE, 2, 250);
Serial.begin(115200); //Sets the baud for serial data transmission
Serial.println("");
Serial.println("=========================");
Serial.println("R2D2 initializing...");
// Inicialitza Bluetooth
Serial.print("Inicialitzant Bluetooth... ");
Serial.println(NOM_BT);
if (INICIALITZAR_BT) {
if (!BT.begin(NOM_BT)) { // name of BT signal
Serial.println("ERROR inicialitzant Bluetooth!");
bt_inicialitzat = 0;
led(RED, 2, 150);
}
else {
Serial.println("Bluetooth Device is Ready to Pair");
bt_inicialitzat = 1;
led(BLUE, 2, 150);
}
}
else
{
Serial.println("INICIALITZAR_BT == FALSE - No inicialitzo Bluetooth");
bt_inicialitzat = 0;
led(BLUE, 4, 150);
}
// Incialitza mp3
dfplayer_init();
// saluda
//delay(1000);
if (dfplayer_inicialitzat) {
myDFPlayer.volume(volum_dfplayer); //Set volume value. From 0 to 30
}
//fesAccio(SOUND_1);
led(BLUE, 1, 500);
// Inicialitza motors
feslogln("Inicialitzant motors...");
// preparar pins motors per pwm
ledcSetup(pwmChannelBracEsq, 30000, 8); // ledcSetup(ledChannel, freq, resolution);
ledcSetup(pwmChannelBracDret, 30000, 8);
ledcSetup(pwmChannelCap, 30000, 8);
ledcAttachPin(pinMotorBracEsqPWM, pwmChannelBracEsq); // ledcAttachPin(ledPin, ledChannel);
ledcAttachPin(pinMotorBracDretPWM, pwmChannelBracDret);
ledcAttachPin(pinMotorCapPWM, pwmChannelCap);
// para els motors
motors_brac_stop(); // te delay
motor_cap_stop();
feslogln("R2D2 - Fi inicialitzacio");
feslogln("=========================");
}
/************************************************
*
* main loop
* inputAccio()
* fesAccio()
*
************************************************
*/
void loop() {
int accioDemanada = -1;
int accioParameter = -1;
// espera accio de l'usuari, es bloquejant
feslogln("r2d2>Waiting for command... ");
accioDemanada = inputAccio();
debug("r2d2> Received: ", accioDemanada);
// instruction expecting parameter
if (accioDemanada > 200) {
debug("r2d2>Waiting for command parameter... ");
accioParameter = inputAccio();
debug("r2d2> Parameter: ", accioParameter);
}
// executa accio. mante el led blau ences mentre la fa <--- ojo power
digitalWrite(LED_BUILTIN, HIGH);
fesAccio (accioDemanada, accioParameter);
debug("r2d2> Command excuted: ", accioDemanada);
digitalWrite(LED_BUILTIN, LOW);
// avisa a BT del resultat
if (bt_inicialitzat) {
BT.println("R2D2_DONE"); // <---- POSAR CODI RETORN
}
}
/*
* ACCIONS, estan composades de comandes primaries
*
*/
void fesAccio(int accio, int param){
switch (accio) {
/*
* CAP
*/
case CAP_ESQUERRA: {
feslogln("CAP_ESQUERRA");
//myDFPlayer.play(1); <--- 23/2/20 fa sorolls (abans no...)
motor_cap(ESQUERRA, 0);
}
break;
case CAP_ESQUERRA_PARAM: {
feslogln("CAP_ESQUERRA", param);
//myDFPlayer.play(1); <--- 23/2/20 fa sorolls (abans no...)
motor_cap(ESQUERRA, param);
}
break;
case CAP_DRETA: {
feslogln("CAP_DRETA");
//myDFPlayer.play(1); <--- 23/2/20 fa sorolls (abans no...)
motor_cap(DRETA, 0);
}
break;
case CAP_DRETA_PARAM: {
feslogln("CAP_DRETA", param);
//myDFPlayer.play(1); <--- 23/2/20 fa sorolls (abans no...)
motor_cap(DRETA, param);
}
break;
case CAP_ESQUERRA_POC: {
feslogln("CAP_ESQUERRA_POC");
motor_cap(ESQUERRA, 400);
}
break;
case CAP_DRETA_POC: {
feslogln("CAP_DRETA_POC");
motor_cap(DRETA, 400);
}
break;
case CAP_MIG: {
feslogln("CAP_MIG");
//myDFPlayer.play(2); <--- 23/2/20 fa sorolls (abans no...)
cap_mig();
}
break;
/*
* LEDS
*/
case LED_VERMELL: {feslogln("LED_VERMELL"); led(RED, 1, 200); } break;
case LED_BLAU: {feslogln("LED_BLAU"); led(BLUE, 1, 200);} break;
case LED_VERMELL_PARAM: {feslogln("LED_VERMELL_PARAM", param); led(RED, param, 200); } break;
case LED_BLAU_PARAM: {feslogln("LED_BLAU_PARAM", param); led(BLUE, param, 200);} break;
case LED_VERMELL_1_SEG: {feslogln("LED_VERMELL_1_SEG"); led(RED, 1, 1000); } break;
case LED_BLAU_1_SEG: {feslogln("LED_BLAU_1_SEG"); led(BLUE, 1, 1000);} break;
/*
* SONS
*/
case SOUND_PLAY_PARAM: {
feslogln("SOUND_PLAY_PARAM", param);
so(0, param);
}
break;
case EFFECTS_PLAY_PARAM: {
feslogln("EFFECTS_PLAY_PARAM", param);
so(1, param);
}
break;
case MUSICA_PLAY_PARAM: {
feslogln("MUSICA_PLAY_PARAM", param);
so(2, param);
}
break;
case FRASE_PLAY_PARAM: {
feslogln("FRASE_PLAY_PARAM", param);
so(3, param);
}
break;
case SOUND_1: {feslogln("SOUND_1"); so(0, 1); } break;
case SOUND_2: {feslogln("SOUND_2"); so(0, 2); } break;
case SOUND_3: {feslogln("SOUND_3"); so(0, 3); } break;
case SOUND_4: {feslogln("SOUND_4"); so(0, 4); } break;
case SOUND_5: {feslogln("SOUND_5"); so(0, 5); } break;
case SOUND_6: {feslogln("SOUND_6"); so(0, 6); } break;
case SOUND_7: {feslogln("SOUND_7"); so(0, 7); } break;
case EFFECTS_NEXT: {feslogln("EFFECTS_NEXT"); so_next(1); } break;
case MUSICA_NEXT: {feslogln("MUSICA_NEXT"); so_next(2); } break;
case FRASE_NEXT: {feslogln("FRASE_NEXT"); so_next(3); } break;
/*
* MOTORS BRAÇOS
*/
case ENDAVANT: {feslogln("ENDAVANT"); motor_bracos_endavant(2000); } break;
case ENDAVANT_PARAM: {feslogln("ENDAVANT_PARAM"); motor_bracos_endavant(param); } break;
case ENDARRERA: {feslogln("ENDARRERA"); motor_bracos_endarrera(2000); } break;
case ENDARRERA_PARAM: {feslogln("ENDARRERA_PARAM"); motor_bracos_endarrera(param); } break;
case GIR_DRETA: {feslogln("GIR_DRETA"); motor_brac(DRETA); } break;
case GIR_ESQUERRA: {feslogln("GIR_ESQUERRA"); motor_brac(ESQUERRA); } break;
case MOTORS_PARAR: {
feslogln("MOTORS_PARAR");
motors_brac_stop();
motor_cap_stop();
}
case VELOCITAT_CAP_PARAM: {
feslogln("VELOCITAT_CAP_PARAM:", param);
velocitat_gir_cap_esq = param;
velocitat_gir_cap_dret = param;
}
break;
case VELOCITAT_ESQ_PARAM: {
feslogln("VELOCITAT_ESQ_PARAM:", param);
velocitat_gir_brac_esq = param;
}
break;
case VELOCITAT_DRET_PARAM: {
feslogln("VELOCITAT_DRET_PARAM:", param);
velocitat_gir_brac_dret = param;
}
break;
/*
* STATUS
*/
case VOLUM_PUJAR: {
feslogln("VOLUM_PUJAR. nou volum:");
volum_dfplayer += 2;
if (volum_dfplayer > 30) volum_dfplayer = 30;
if (dfplayer_inicialitzat) {
myDFPlayer.volume(volum_dfplayer); //Set volume value. From 0 to 30
}
feslogln(volum_dfplayer);
}
break;
case VOLUM_BAIXAR: {
feslogln("VOLUM_BAIXAR. nou volum:");
volum_dfplayer -= 2;
if (volum_dfplayer<0) volum_dfplayer = 0;
if (dfplayer_inicialitzat) {
myDFPlayer.volume(volum_dfplayer); //Set volume value. From 0 to 30
}
feslogln(volum_dfplayer);
}
break;
break;
case DEBUG_ON: {
feslogln("DEBUG_ON");
debug_on = 1;
}
break;
case DEBUG_OFF: {
feslogln("DEBUG_OFF");
debug_on = 0;
}
break;
case SHOW_STATUS: {
feslogln("R2D2 STATUS:");
feslogln("VERSIO:", VERSIO);
feslogln("dfplayer_inicialitzat:", dfplayer_inicialitzat);
feslogln("myDFPlayer.available():", myDFPlayer.available());
if (myDFPlayer.available()) {
dfplayer_printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Print the detail message from DFPlayer to handle different errors and states.
}
feslogln("debug_on:", debug_on);
feslogln("volum_dfplayer:", volum_dfplayer);
feslogln("velocitat_gir_cap_esq:", velocitat_gir_cap_esq);
feslogln("velocitat_gir_cap_dret:", velocitat_gir_cap_dret);
feslogln("velocitat_gir_brac_esq:", velocitat_gir_brac_esq);
feslogln("velocitat_gir_brac_dret:", velocitat_gir_brac_dret);
feslogln("velocitat_gir_cap_esq:", velocitat_gir_cap_esq);
feslogln("digitalRead(pinCapEsq):", digitalRead(pinCapEsq));
feslogln("digitalRead(pinCapMig):", digitalRead(pinCapMig));
feslogln("digitalRead(pinCapDret):", digitalRead(pinCapDret));
feslogln("digitalRead(pinIntPit):", digitalRead(pinIntPit));
feslogln("digitalRead(pinPeu):", digitalRead(pinPeu));
feslogln("sd_folder_files[0]=", sd_folder_files[0] );
feslogln("sd_folder_files[1]=", sd_folder_files[1] );
feslogln("sd_folder_files[2]=", sd_folder_files[2] );
feslogln("sd_sound_next[0]=", sd_sound_next[0]);
feslogln("sd_sound_next[1]=", sd_sound_next[1]);
feslogln("sd_sound_next[2]=", sd_sound_next[2]);
}
break;
default: {
feslogln("Unknow action!");
led(RED, 5, 50);
}
}
}
/*
* Espera fins que rebi aluguna comanda d'accio
* Pot ser per interruptor del pit o per bluetooth
* Interruptor del mig torna SOUND_NEXT_FOLDER_1
* Bluetooth torna una de les instruccions
* Mentre espera input avisa per debug si reb algun input dels sensors (nomes un cop)
*
*/
int inputAccio(){
int acc = NULL;
char dataBluetooth = 0; //Variable for storing received data en BT
char txt[20];
int compt_avis = 0; // en idle fa delay 50 ms, cada 20*10 = 10 segs enviarem msg
// simplificar aixo!!!
bool capDretAvisat = false;
bool capEsqAvisat = false;
bool capMigAvisat = false;
bool peuAvisat = false;
// esperar alguna comanda
do {
// 1a. si hi ha entrada de bluetooth
if ((bt_inicialitzat) && (BT.available() > 0))
{
dataBluetooth = llegeix_bluetooth();
if (dataBluetooth != -1) {
acc = dataBluetooth;
}
}
// 1b. si hi ha entrada de serial
else if(Serial.available() > 0)
{
//acc = Serial.read();
acc = Serial.parseInt();
// el seguent sera un non char - cal treure. o que passa si poso una lletra <----
//while Serial.read()=='\n' || Serial.read()
// if (c1!= '\n' && c1 != '\r') {
}
// 2. si s'ha apretat el boto del pit
else if ((BOTO_PIT_CONNECTAT != 0) && (digitalRead(pinIntPit)==HIGH)) {
delay(300); // eliminar rebots <--- REVISAR que no s'acumulin molts retards
debug("pinIntPit pressed");
acc = MUSICA_NEXT;
}
// 3. si detecta algun sensor ho diu per bluetooth (per testejar connexions)
else if (digitalRead(pinCapEsq)== HIGH and !capEsqAvisat){
debug("pinCapEsq pressed");
peuAvisat = false;
capEsqAvisat = true;
capMigAvisat = false;
capDretAvisat = false;
}
else if (digitalRead(pinCapMig)== HIGH and !capMigAvisat){
debug("pinCapMig pressed");
peuAvisat = false;
capEsqAvisat = false;
capMigAvisat = true;
capDretAvisat = false;
}
else if (digitalRead(pinCapDret)== HIGH and !capDretAvisat){
debug("pinCapDret pressed");
peuAvisat = false;
capEsqAvisat = false;
capMigAvisat = false;
capDretAvisat = true;
}
else if (digitalRead(pinPeu)==HIGH and !peuAvisat){
debug("pinPeu pressed");
peuAvisat = true;
capEsqAvisat = false;
capDretAvisat = false;
capMigAvisat = false;
}
else {
delay(50);
compt_avis+=1;
if (compt_avis==200){ // 50ms * 200 = 10 segs
debug("r2d2>Waiting for command... ");
compt_avis = 0;
}
}
} while (acc == NULL);
return acc;
}
/*
* llegeix un int del bluetooth.
* pot ser de 1, 2 o 3 digits. talla quan troba algo que no es digit.
* quan ha acabat consumeix \n o \r que vinguin per darrera
* torna el numero o -1 si el missatge no comença amb número
*
*/
int llegeix_bluetooth(){
int dataBluetooth = -1;
char c1, c2, c3;
char buf[2];
// llegeix primer caracter
if (bt_inicialitzat) {
if (BT.available()){
c1 = BT.read();
delay(2); //slow looping to allow buffer to fill with next character
// si es digit, el guarda i intenta llegir-ne un altre
if (isDigit(c1)) {
buf[0]=c1;
buf[1]='\0';
dataBluetooth = atoi(buf);
// mirem si hi ha un segon digit
if (BT.available()){
c2 = BT.read();
delay(2); //slow looping to allow buffer to fill with next character
if (isDigit(c2)) {
buf[0]=c2;
buf[1]='\0';
dataBluetooth = dataBluetooth*10 + atoi(buf);
// mirem si hi ha un tercer digit
if (BT.available()){
c3 = BT.read();
delay(2); //slow looping to allow buffer to fill with next character
if (isDigit(c3)) {
buf[0]=c3;
buf[1]='\0';
dataBluetooth = dataBluetooth*10 + atoi(buf);
}
}
}
}
}
if (dataBluetooth != -1) {
debug("BT rebut: ", dataBluetooth);
}
// si venen CR o LF els consumeix
while (BT.available() && (BT.peek()=='\n' || BT.peek()=='\r')){
c1 = BT.read();
delay(2); //slow looping to allow buffer to fill with next character
}
}
}
return dataBluetooth;
}
/**********************************************
*
* CAP
*
**********************************************
*/
void cap_mig()
{
debug("entro a cap_mig()");
bool socAlMig = (digitalRead(pinCapMig) == HIGH);
bool socAlExtrem;
bool girarDreta;
// si ja esta al mig no fem res
while (!socAlMig){
// si som a l'esquerra girem cap a la dreta, en qualsevol altre posicio girem cap a l'esquerra
girarDreta = (digitalRead(pinCapEsq)==HIGH);
if (girarDreta){
motor_cap(DRETA, -1);
}
else {
motor_cap(ESQUERRA, -1);
}
// continuem fins que passem pel mig o arribem a alguna de les puntes (pot ser que ens haguem equivocat
// a l'hora de triar cap a on girar, llavors girarem cap a l'altre costat i passarem segur pel mig)
do {
delay(5);
socAlMig = (digitalRead(pinCapMig) == HIGH);
// per detectar extrem he de saber cap a on gira perque pot ser que estigui a l'extrem perque encara no ha començat a girar
socAlExtrem = (digitalRead(pinCapEsq)==HIGH and !girarDreta) or (digitalRead(pinCapDret)==HIGH and girarDreta);
} while (!socAlMig and !socAlExtrem);
motor_cap_stop();
}
}
/***************************************************
*
* MOTORS BRAÇOS
*
***************************************************
*/
void motor_brac(direccio d){
if (d == DRETA) {
debug("motor_brac DRETA");
ledcWrite(pwmChannelBracDret, velocitat_gir_brac_dret);
digitalWrite(pinMotorBracDretDIR,HIGH); // forward?
}
else {
debug("motor_brac ESQUERRA");
ledcWrite(pwmChannelBracEsq, velocitat_gir_brac_esq);
digitalWrite(pinMotorBracEsqDIR,HIGH); // forward
}
motor_bracos_funcionant(TEMPS_GIR);
}
void motor_bracos_endavant(int ms){
debug("motor_bracos_endavant");
ledcWrite(pwmChannelBracDret, velocitat_gir_brac_dret);
digitalWrite(pinMotorBracDretDIR,HIGH); // forward?
ledcWrite(pwmChannelBracEsq, velocitat_gir_brac_esq);
digitalWrite(pinMotorBracEsqDIR,HIGH); // forward
motor_bracos_funcionant(ms);
}
void motor_bracos_endarrera(int ms){
debug("motor_bracos_endavant");
ledcWrite(pwmChannelBracDret, 255-velocitat_gir_brac_dret);
digitalWrite(pinMotorBracDretDIR,LOW); // forward?
ledcWrite(pwmChannelBracEsq, 255-velocitat_gir_brac_esq);
digitalWrite(pinMotorBracEsqDIR,LOW); // forward
motor_bracos_funcionant(ms);
}
/*
* mante els motors en funcionament ms milisegons i els para
* si ms == 0 fins que s'apreta boto del peu
* si s'apreta el boto del peu es para
*/
void motor_bracos_funcionant(int ms){
if (digitalRead(pinPeu)==LOW){
if (ms==0) {
while (digitalRead(pinPeu)==LOW);
}
else {
float bucles = ms/5;
do {
delay(5);
bucles--;
} while (bucles>0 && digitalRead(pinPeu)==LOW);
}
}
motors_brac_stop();
}
void motors_brac_stop()
{
debug("motors_brac_stop");
ledcWrite(pwmChannelBracEsq,0);
digitalWrite(pinMotorBracEsqDIR,LOW);
ledcWrite(pwmChannelBracDret,0);
digitalWrite(pinMotorBracDretDIR,LOW);
//delay(500);
delay(50);
}
/***************************************************
*
* MOTOR CAP
*
***************************************************
*/
/*
* Mou el cap en la direccio d, durant ms milisegons, si arriba al final para
* si ms == 0 para si arriba al final
* si ms == -1 para si arriba al final o passa pel mig
*/
void motor_cap(direccio d, int ms){
if (d == DRETA) {
debug("motor_cap(DRETA)");
debug(ms);
ledcWrite(pwmChannelCap, 255 - velocitat_gir_cap_dret); // backward
digitalWrite(pinMotorCapDIR,LOW); // backward
}
else {
debug("motor_cap(ESQUERRA)");
debug(ms);
ledcWrite(pwmChannelCap, velocitat_gir_cap_esq); // forward
digitalWrite(pinMotorCapDIR,HIGH); // forward
}
motor_cap_funcionant(d, ms);
}
void motor_cap_stop(){
debug("motor_cap_stop()");
ledcWrite(pwmChannelCap,0);
digitalWrite(pinMotorCapDIR,LOW);
//delay(500);
delay(50);
}
/*
* mante els motors en funcionament ms milisegons i els para
* si ms == 0 fins que fa tope a l'extrem
* si ms == -1 fins que fa tope a l'extrem o al mig
* si s'apreta el boto del peu es para
*/
void motor_cap_funcionant(direccio d, int ms){
int pinCap;
if (d==DRETA) {
pinCap = pinCapDret;
}
else {
pinCap = pinCapEsq;
}
if (ms==0) {
while (digitalRead(pinCap)==LOW);
}
else if (ms==-1) {
while (digitalRead(pinCapMig)==LOW && digitalRead(pinCap)==LOW );
}
else {
float bucles = ms/5;
do {
delay(5);
bucles--;
} while (bucles>0 && digitalRead(pinCap)==LOW);
}
motor_cap_stop();
}
/*********************************************
*
* LEDS
*
**********************************************
*/
void led(color_led col, int repeticions, int ms) {
int pinLed;
if (col == RED) {
debug("led red times=", repeticions);
debug("ms=", ms);
pinLed = pinLedVermell;
}
else {
debug("led blue ms:", ms);
pinLed = pinLedBlau;
}
for (int i=0;i<=repeticions;i++) {
digitalWrite(pinLed, LOW);
delay(ms);
digitalWrite(pinLed, HIGH);
delay(50);
}
}
/********************************************
*
* MISSATGES
*
* no fer servir print sense ln perque em bloqueja el BT
* ******************************************
*/
void debug(char* txt, int n)
{
if (debug_on) {
feslogln(txt, n);
}
}
void debug(char *txt)
{
if (debug_on) {
feslogln(txt);
}
}
void debug(int n)
{
if (debug_on) {
feslogln(n);
}
}
// OJO SI TXT ES > 200 CHARS <---
void feslogln(char *txt, int n)
{
char st[200];
sprintf(st,"%s %u",txt,n);
feslogln(st);
}
void feslogln(char *txt)
{
Serial.println(txt);
if (bt_inicialitzat) {
BT.println(txt);
}
}
void feslogln(int n)
{
Serial.println(n);
if (bt_inicialitzat) {
BT.println(n);
}
}
/*****************************************
*
* SO
* - 8/3/20 -- HO POSO TOT A L'ARREL PER QUE ALGO NO FUNCIONA AL FOLDER <------
* - directori 1: sons
* - directori 2: musiques
* - directori 3: frases
*****************************************
*/
void dfplayer_init() {
feslogln("");
feslogln("Initializing DFPlayer ... (May take 3~5 seconds)");
mySoftwareSerial.begin(9600, SERIAL_8N1, pinRX2, pinTX2); // Serial2.begin(baud-rate, protocol, RX pin, TX pin);
//29/2/20 - segueix fent sorolls sense el false, el poso a veure que passa
// 23/2/20 - amb el false el mp3 es sentia pero hi havia sorolls al principi. si no hi ha el mp3 no detecta l'error
if (INICIALITZAR_DFPLAYER==1) {
//if (!myDFPlayer.begin(mySoftwareSerial)) { //Use softwareSerial to communicate with mp3.
if (!myDFPlayer.begin(mySoftwareSerial, false)) { //Use softwareSerial to communicate with mp3.
feslogln("*** ERROR inicialitzant DFPlayer *** ");
feslogln("... pero el programa continua com si estes inicialitzat....");
dfplayer_inicialitzat = 1;
led(RED, 2, 200);
}
else
{
dfplayer_inicialitzat = 1;
led(BLUE, 2, 200);
}
feslogln("DFPlayer Mini online.");
}
else {
feslogln("INICIALITZAR_DFPLAYER == FALSE - No inicialitzo DFPlayer");
}
sd_sound_next[0] = 0;
sd_sound_next[1] = 0;
sd_sound_next[2] = 0;
}
void so(int folder, int sound)
{
debug("so folder * 1000 + sound:", folder * 1000 + sound);
if (dfplayer_inicialitzat) {
if (folder==0) {
myDFPlayer.play(sound);
}
else {
myDFPlayer.playFolder(folder, sound);
}
if (myDFPlayer.available()) {
dfplayer_printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Print the detail message from DFPlayer to handle different errors and states.
}
}
}
// fa next del folder indicat
// caldria comprovar que folder < MAX_FOLDERS_SD
void so_next(int folder)
{
debug("so_next folder: folder*1000 + sd_sound_next", folder*1000+sd_sound_next[folder-1] );
// inicialitza si cal. no ho fem al init perque iguan no esta inicialitzat el dfplayer
if (sd_sound_next[folder-1]==0) {
// llegeix numero d'arxius per folder <--- GENERALITZAR SI CAL
if (myDFPlayer.available()) {
sd_folder_files[0] = myDFPlayer.readFileCountsInFolder(1);
sd_folder_files[1] = myDFPlayer.readFileCountsInFolder(2);
sd_folder_files[2] = myDFPlayer.readFileCountsInFolder(3);
sd_sound_next[0] = 1; // CALDRIA COMPROVAR QUE HI HA ALGUN FITXER <----
sd_sound_next[1] = 1; // CALDRIA COMPROVAR QUE HI HA ALGUN FITXER <----
sd_sound_next[2] = 1; // CALDRIA COMPROVAR QUE HI HA ALGUN FITXER <----
}
else {
feslogln("so_next(): No puc llegir les carpetes del dfplayer");
if (myDFPlayer.available()) {
dfplayer_printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Print the detail message from DFPlayer to handle different errors and states.
}
}
}
if (dfplayer_inicialitzat) {
myDFPlayer.playFolder(folder, sd_sound_next[folder-1]);
if (myDFPlayer.available()) {
dfplayer_printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Print the detail message from DFPlayer to handle different errors and states.
}
}
if (sd_sound_next[folder-1] > sd_folder_files[folder-1]){
sd_sound_next[folder-1] = 1;
}
}
void dfplayer_printDetail(uint8_t type, int value){
switch (type) {
case TimeOut:
feslogln("Time Out!");
break;
case WrongStack:
feslogln("Stack Wrong!");
break;
case DFPlayerCardInserted:
feslogln("Card Inserted!");
break;
case DFPlayerCardRemoved:
feslogln("Card Removed!");
break;
case DFPlayerCardOnline:
feslogln("Card Online!");
break;
case DFPlayerUSBInserted:
feslogln("USB Inserted!");
break;
case DFPlayerUSBRemoved:
feslogln("USB Removed!");
break;
case DFPlayerPlayFinished:
feslogln("Number:");
feslogln(value);
feslogln(" Play Finished!");
break;
case DFPlayerError:
feslogln("DFPlayerError:");
switch (value) {
case Busy:
feslogln("Card not found");
break;
case Sleeping:
feslogln("Sleeping");
break;
case SerialWrongStack:
feslogln("Get Wrong Stack");
break;
case CheckSumNotMatch:
feslogln("Check Sum Not Match");
break;
case FileIndexOut:
feslogln("File Index Out of Bound");
break;
case FileMismatch:
feslogln("Cannot Find File");
break;
case Advertise:
feslogln("In Advertise");
break;
default:
break;
}
break;
default:
break;
}
}