Buenas! Estoy aprendiendo a usar la librería Protothreads aplicandola a un proyecto simple que es mover un auto con 4 motores de corriente continua para que busque una pelota. Se que se puede hacer sin el uso de la misma pero quiero aprender en caso de necesitarla más adelante. Mi problema es que no obtengo lecturas de los potenciometros que estoy usando para simular una posición X e Y (xsim e ysim en el codigo) ni tampoco los comandos por Bluetooth (modificar estados A B y C pulsando botones). He revisado y contrastado con otros ejemplos que encontré online pero no logro ver el problema. (El hardware esta testeado con otro codigo y funciona correctamente) Las variables yCAM y xCAM corresponderia a agregar una camara y recibir señales de la misma. No agregue todavia las comparaciones entre xsim e ysim con xCAM e yCAM porque no logro hacer funcionar lo que tengo hasta ahora. En el serial aparece "Estado=? x=0.00 y=0.00" Un saludo y gracias de antemano!
#include <protothreads.h>
#include <SoftwareSerial.h>
#define RxD 9
#define TxD 2
#define xCAM1 153
#define xCAM2 163
#define yCAM 180
enum STATE {A, B, C};
float xsim=analogRead(A1);
float ysim=analogRead(A2);
//Variables BT
int Estado=0;
SoftwareSerial BTCOM (RxD,TxD);
char nombreBT[10]="JuanBT123";
char velocidad='4';
char pin[5]="0000";
//Variables motores
int ENA=3, ENB=6, IN1=4, IN2=5, IN3=7, IN4=8;
//Protothreads
static struct pt pt1;
static struct pt pt2;
static struct pt pt3;
static struct pt cond;
static struct pt wt;
//Funciones de los hilos
static int Display (struct pt *pt){
PT_BEGIN(pt);
static unsigned long Tiempo_inicio=0;
while(1){
Tiempo_inicio=millis();
PT_WAIT_UNTIL(pt, millis()-Tiempo_inicio>1000);
Serial.print("Estado= ");
Serial.write(Estado);
Serial.print("x= ");
Serial.print(xsim);
Serial.print(" y= ");
Serial.println(ysim);
Tiempo_inicio=millis();
}
PT_END(pt);
}
static int Buscar (struct pt *pt){
PT_BEGIN(pt);
while(1){
digitalWrite(IN2,HIGH);
digitalWrite(IN1,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
analogWrite(ENB,80);
analogWrite(ENA,80);
}
PT_END(pt);
}
static int Apagar (struct pt *pt){
PT_BEGIN(pt);
while(1){
digitalWrite(IN2,LOW);
digitalWrite(IN1,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
analogWrite(ENB,0);
analogWrite(ENA,0);
}
PT_END(pt);
}
static int Avance (struct pt *pt){
PT_BEGIN(pt);
while(1){
digitalWrite(IN2,HIGH);
digitalWrite(IN1,LOW);
digitalWrite(IN4,HIGH);
digitalWrite(IN3,LOW);
analogWrite(ENB,200);
analogWrite(ENA,200);
}
PT_END(pt);
}
static int Conductor (struct pt *pt){
PT_BEGIN(pt);
while(1){
if (BTCOM.available()>0);
Estado=BTCOM.read();
Display(&wt);
if(Estado==A){
Buscar(&pt1);
}
if(Estado==B){
Apagar(&pt2);
}
if(Estado==C){
Avance(&pt3);
}
}
PT_END(pt);
}
void setup() {
//Módulo Bluetooth
BTCOM.flush();
BTCOM.begin(9600);
Serial.begin(9600);
BTCOM.write("AT");
delay(1000);
BTCOM.write("AT+NAME");
BTCOM.write(nombreBT);
delay(1000);
BTCOM.write("AT+BAUD");
BTCOM.write(velocidad);
BTCOM.write("AT+PIN");
BTCOM.write(pin);
delay(1000);
//Pines para los motores
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(ENB,OUTPUT);
//Inicializo Protothreads
PT_INIT(&pt1);
PT_INIT(&pt2);
PT_INIT(&pt3);
PT_INIT(&cond);
PT_INIT(&wt);
}
void loop() {
Conductor(&cond);
Avance(&pt3);
Buscar(&pt1);
Display(&wt);
Apagar(&pt2);
}