bluetooth, at last

include/sensors/data_source_bt.h

@@ -8,8 +8,13 @@ class DataSourceBT : public DataSource{
     public:
         DataSourceBT(HardwareSerial* ser, int baud);
         void test() override;
+        void update() override;
         void connect();
-        void reconnect();
+        void receive();
+        void send();
 
-        bool b, comrade;
+        bool can_bombard, bt_bombarded, comrade;
+        unsigned long bt_timer, last_received, t;
+
+        char received, tosend;
 };

include/strategy_roles/games.h

@@ -10,10 +10,10 @@
 #include "strategy_roles/game.h"
 #include "strategy_roles/striker.h"
 #include "strategy_roles/precision_shooter.h"
-#include "strategy_roles/keeper.h"
+// #include "strategy_roles/keeper.h"
 
 void initGames();
 
 g_extr Game* striker;
 g_extr Game* precision_shooter;
-g_extr Game* keeper;
+// g_extr Game* keeper;

src/main.cpp

@@ -17,6 +17,7 @@ bool striker_condition = false;
 bool keeper_condition = false;
 
 void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
   pinMode(BUZZER, OUTPUT);
   tone(BUZZER, 220, 250);
   delay(1500);
@@ -47,18 +48,15 @@ void setup() {
 }
 
 void loop() {
-  DEBUG.println("aaaa");
   updateSensors();
-
   drive->resetDrive();
   
   striker_condition = role == HIGH;
-  keeper_condition = role == LOW;
-
   striker->play(striker_condition);
-  // keeper->play(keeper_condition);
   // precision_shooter->play(1);
 
+  // keeper_condition = role == LOW;
+  // keeper->play(keeper_condition);
   // testmenu->testMenu();
 
   // // Last thing to do: movement and update status vector

src/sensors/data_source_bt.cpp

@@ -1,33 +1,82 @@
 #include "sensors/data_source_bt.h"
 
-//if needed, substitute Serial1 with Serial3 to return to the old code setup
+/*Currently using a master-slave setup
+One bt has been setup using
+SM,3 - Master mode
+SA, 0 - Disable authentication
+SR, xxxx - Address of the other bt
+SU,96 - baud rate 9600
+SN, name - Name for clarity
+
+The other one
+SM,0 - Master mode
+SA, 0 - Disable authentication
+SU,96 - baud rate 9600
+SN, name - Name for clarity
+ST, 5 - Configuration timeout
+
+For now it seems that a slave can easily recovery from a master restart, but not the opposite. We will make sure that doesn't happen
+*/
+
 
 DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
-    // connect();
+    bt_timer = millis();
+    can_bombard = false;
+    bt_bombarded = false;
+
+    comrade = false;
+
+    last_received = 0;
+    t = 0;
+
+    tosend = 'B';
+
+    connect();
 }
 
 void DataSourceBT :: connect(){
-    Serial1.print("$");
-    Serial1.print("$");
-    Serial1.print("$");
-    delay(100);
-    Serial1.println("C");
+  //Give the bt time to be brought up (about 5-6 secs.)
+  //When turned on the bt onboard led will blink. After 5-6 secs it will start blinking at a lower freq. We need to wait for that to happen
+  // if(millis() >= bt_timer + 5000 && !bt_bombarded){
+  //   can_bombard = true;
+  // }
+
+  // //For a sec, bombard the other bt of packets. Since we are in sm2 this will make them connect. Note that the two of them cannot accept connections while they're trying to connect
+  // if(can_bombard && millis() >= bt_timer + 5000 && millis() <= bt_timer + 6000){
+  //   DEBUG.println("Bombarding");
+  //   Serial1.print(tosend);
+  //   bt_bombarded = true;
+  // }else{
+  //   can_bombard = false;
+  // }
+
 }
 
-void DataSourceBT :: reconnect(){
-    if(!comrade){
-    if(!b){
-      Serial1.print("$");
-      Serial1.print("$");
-      Serial1.print("$");
-    }else{
-      Serial1.println("C");
-    }
-
-    b = !b;
-  }else{
-    Serial1.println("---");
+void DataSourceBT :: receive(){
+  while(Serial1.available()) {
+    last_received = millis();
+    received = (char) Serial1.read();
+    comrade = true;
+    DEBUG.println(received);
   }
+  if(millis() - last_received > 2000) 
+  comrade = false;
+}
+
+void DataSourceBT::send(){
+  if(millis() - t >= 250){
+    Serial1.print(tosend);
+    // DEBUG.print("Sending: ");
+    DEBUG.println(tosend);
+  }
+}
+
+void DataSourceBT::update(){
+  // if(!bt_bombarded && can_bombard) connect();
+  receive();
+  send();
+  digitalWriteFast(LED_BUILTIN, comrade);
+  digitalWriteFast(BUZZER, received == 'B') ;
 }
 
 void DataSourceBT :: test(){

src/sensors/sensors.cpp

@@ -18,7 +18,7 @@ void initSensors(){
     camera = new DataSourceCameraConic(&Serial3, 19200);
     // tone(BUZZER, 285, 250);
     // delay(350);
-    // bt = new DataSourceBT(&Serial1, 115200);
+    bt = new DataSourceBT(&Serial1, 9600);
     roller = new Roller(30, 31, 1000, 2000, 500);
  }
 
@@ -32,5 +32,7 @@ void updateSensors(){
     ball->update();
     camera->update();
 
+    bt->update();
+
     roller->update();
 }