Revert "first commit, starting to work with a new setup"

This reverts commit e341214905.

.vscode/extensions.json

@@ -3,5 +3,8 @@
     // for the documentation about the extensions.json format
     "recommendations": [
         "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
     ]
 }

include/sensors/sensors.h

@@ -22,7 +22,7 @@
 #include "sensors/data_source_bno055.h"
 #include "sensors/data_source_camera_conicmirror.h"
 #include "sensors/data_source_camera_vshapedmirror.h"
-
+#include "vars.h"
 
 void initSensors();
 void updateSensors();
@@ -39,6 +39,5 @@ s_extr DataSourceBT* bt;
 s_extr DataSourceBallPresence* ballPresence;
 
 s_extr int role;
-s_extr int robot_indentifier;
 
 s_extr Roller* roller;

include/systems/lines/linesys_camera.h

@@ -7,14 +7,14 @@
 
 #include "vars.h" 
 
-#define S1O A7
+#define S1I A14
-#define S1I A6
+#define S1O A15
-#define S2O A2
+#define S2I A16
-#define S2I A3
+#define S2O A17
-#define S3I A9
+#define S3I A20
-#define S3O A8
+#define S3O A0
-#define S4I A0
+#define S4I A1
-#define S4O A1
+#define S4O A2
 
 #define LINE_THRESH_CAM 350
 #define EXIT_TIME 300

include/vars.h

@@ -9,22 +9,12 @@
 
 #define GLOBAL_SPD_MULT 1.0
 
-/*SWS and LEDS are to be tested and implemented in the code. 
-the new setup is the one commented, leds have to be written 
-in the 32U4 code*/
-
 #define LED_R 20
 #define LED_Y 17
 #define LED_G 13
 
-#define BUZZER 6
+#define BUZZER 30
-#define SWITCH_SX 39
+#define SWITCH_SX 28
-#define SWITCH_DX 38
+#define SWITCH_DX 29
-#define SWITCH_ID 33
-
-#define ROLLER_INA 34
-#define ROLLER_INB 35
-
-#define BALL_32U4 Serial2
 
 extr float sins[360], cosins[360];

src/main.cpp

@@ -53,15 +53,15 @@ void loop() {
   // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   // keeper_condition = role == LOW;
 
-  if(robot_indentifier){
+  // if(robot_indentifier){
-    tc3_2->play(role);
+  //   tc3_2->play(role);
-    tc3_1->play(!role);
+  //   tc3_1->play(!role);
 
-    // Last thing to do: movement and update status vector
+  //   // Last thing to do: movement and update status vector
-    drive->drivePrepared();  
+  //   drive->drivePrepared();  
-  }else{
+  // }else{
-    drive->stopAll();
+  //   drive->stopAll();
-  }
+  // }
 
   testmenu->testMenu();
   updateStatusVector();

src/sensors/data_source_bt.cpp

@@ -1,24 +1,5 @@
 #include "sensors/data_source_bt.h"
 
-/*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){
     bt_timer = millis();
     bt_bombarded = false;
@@ -36,32 +17,30 @@ DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_,
 }
 
 void DataSourceBT :: connect(){
-  //Give the bt time to be brought up (about 5-6 secs.)
+    Serial3.print("$");
-  //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
+    Serial3.print("$");
-  // if(millis() >= bt_timer + 5000 && !bt_bombarded){
+    Serial3.print("$");
-  //   can_bombard = true;
+    delay(100);
-  // }
+    Serial3.println("C");
+}
 
-  // //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
+// void DataSourceBT :: reconnect(){
-  // if(can_bombard && millis() >= bt_timer + 5000 && millis() <= bt_timer + 6000){
+//     if(!comrade){
-  //   DEBUG.println("Bombarding");
+//     if(!b){
-  //   Serial1.print(tosend);
+//       Serial3.print("$");
-  //   bt_bombarded = true;
+//       Serial3.print("$");
+//       Serial3.print("$");
 //     }else{
-  //   can_bombard = false;
+//       Serial3.println("C");
 //     }
 
-}
+//     b = !b;
-
+//   }else{
-void DataSourceBT :: receive(){
+//     Serial3.println("---");
-  while(Serial1.available()) {
+//   }
-    last_received = millis();
+//   if(millis() - last_received > 2000) 
-    received = (char) Serial1.read();
+//   comrade = false;
-    comrade = true;
+// }
-  }
-  if(millis() - last_received > 2000) 
-  comrade = false;
-}
 
 void DataSourceBT::send(){
   if(millis() - t >= 250 && can_send ){
@@ -80,7 +59,7 @@ void DataSourceBT :: test(){
     if (DEBUG.available()) {
     DEBUG.println((char)DEBUG.read());
     }
-    if (Serial1.available()) {
+    if (Serial3.available()) {
-    Serial1.write((char)Serial1.read());
+    Serial3.write((char)Serial3.read());
   }
 }

src/sensors/data_source_camera_conicmirror.cpp

@@ -196,10 +196,10 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xy_fixed;
   }
 
-  byte to_32u4 = 0;
+  // byte to_32u4 = 0;
-  to_32u4 |= (CURRENT_DATA_READ.ySeen);
+  // to_32u4 |= (CURRENT_DATA_READ.ySeen);
-  to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
+  // to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
-  BALL_32U4.write(to_32u4);
+  // BALL_32U4.write(to_32u4);
 }
 
 void DataSourceCameraConic::test(){

src/sensors/sensors.cpp

@@ -2,35 +2,27 @@
 #include "sensors/sensors.h"
 
 void initSensors(){
-    pinMode(SWITCH_1, INPUT);
+    pinMode(SWITCH_SX, INPUT);
-    pinMode(SWITCH_2, INPUT);
+    pinMode(SWITCH_DX, INPUT);
 
     pinMode(LED_R, OUTPUT);
     pinMode(LED_Y, OUTPUT);
     pinMode(LED_G, OUTPUT);
 
-    drive = new DriveController(new Motor(12, 11, 4, 55), new  Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
+    // dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) };
-    // tone(BUZZER, 270, 250);
+
-    // delay(350);
+    drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
-    // tone(BUZZER, 275, 250);
+    ball = new DataSourceBall(&Serial4, 57600);
-    // delay(350);
+    camera = new DataSourceCameraConic(&Serial2, 19200);
-    ball = new DataSourceBall(&BALL_32U4, 57600);
+    // usCtrl = new DataSourceCtrl(dUs);
-    // tone(BUZZER, 280, 250);
+    bt = new DataSourceBT(&Serial3, 115200);
-    // delay(350);
-    camera = new DataSourceCameraConic(&Serial3, 19200);
-    // tone(BUZZER, 285, 250);
-    // delay(350);
-    bt = new DataSourceBT(&Serial1, 9600);
-    roller = new Roller(30, 31, 1000, 2000, 500);
-    ballPresence = new DataSourceBallPresence(A13, true);
  }
 
 void updateSensors(){
     role = digitalRead(SWITCH_DX);
     camera->old_goalOrientation = camera ->goalOrientation;
     camera->goalOrientation = digitalRead(SWITCH_SX);
-    robot_indentifier = digitalRead(SWITCH_ID);
     
     compass->update();
     ball->update();
@@ -39,5 +31,5 @@ void updateSensors(){
 
     bt->update();
 
-    roller->update();
+    // roller->update();
 }