diff --git a/.vscode/extensions.json b/.vscode/extensions.json
index 0f0d740..080e70d 100755
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -1,7 +1,10 @@
-{
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
-    "recommendations": [
-        "platformio.platformio-ide"
-    ]
-}
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}
diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h
index 390ffa7..a144e98 100644
--- a/include/sensors/sensors.h
+++ b/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;
\ No newline at end of file
diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h
index 94c60ff..27ed37f 100644
--- a/include/systems/lines/linesys_camera.h
+++ b/include/systems/lines/linesys_camera.h
@@ -7,14 +7,14 @@
 
 #include "vars.h" 
 
-#define S1O A7
-#define S1I A6
-#define S2O A2
-#define S2I A3
-#define S3I A9
-#define S3O A8
-#define S4I A0
-#define S4O A1
+#define S1I A14
+#define S1O A15
+#define S2I A16
+#define S2O A17
+#define S3I A20
+#define S3O A0
+#define S4I A1
+#define S4O A2
 
 #define LINE_THRESH_CAM 350
 #define EXIT_TIME 300
diff --git a/include/vars.h b/include/vars.h
index 097a8e1..4f6e9a9 100755
--- a/include/vars.h
+++ b/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 SWITCH_SX 39
-#define SWITCH_DX 38
-#define SWITCH_ID 33
-
-#define ROLLER_INA 34
-#define ROLLER_INB 35
-
-#define BALL_32U4 Serial2
+#define BUZZER 30
+#define SWITCH_SX 28
+#define SWITCH_DX 29
 
 extr float sins[360], cosins[360];
diff --git a/src/main.cpp b/src/main.cpp
index 624df7b..108ca10 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -53,15 +53,15 @@ void loop() {
   // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   // keeper_condition = role == LOW;
 
-  if(robot_indentifier){
-    tc3_2->play(role);
-    tc3_1->play(!role);
+  // if(robot_indentifier){
+  //   tc3_2->play(role);
+  //   tc3_1->play(!role);
 
-    // Last thing to do: movement and update status vector
-    drive->drivePrepared();  
-  }else{
-    drive->stopAll();
-  }
+  //   // Last thing to do: movement and update status vector
+  //   drive->drivePrepared();  
+  // }else{
+  //   drive->stopAll();
+  // }
 
   testmenu->testMenu();
   updateStatusVector();
diff --git a/src/sensors/data_source_bt.cpp b/src/sensors/data_source_bt.cpp
index 1e6e7d7..b050c98 100644
--- a/src/sensors/data_source_bt.cpp
+++ b/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.)
-  //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;
-  // }
-
+    Serial3.print("$");
+    Serial3.print("$");
+    Serial3.print("$");
+    delay(100);
+    Serial3.println("C");
 }
 
-void DataSourceBT :: receive(){
-  while(Serial1.available()) {
-    last_received = millis();
-    received = (char) Serial1.read();
-    comrade = true;
-  }
-  if(millis() - last_received > 2000) 
-  comrade = false;
-}
+// void DataSourceBT :: reconnect(){
+//     if(!comrade){
+//     if(!b){
+//       Serial3.print("$");
+//       Serial3.print("$");
+//       Serial3.print("$");
+//     }else{
+//       Serial3.println("C");
+//     }
+
+//     b = !b;
+//   }else{
+//     Serial3.println("---");
+//   }
+//   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()) {
-    Serial1.write((char)Serial1.read());
+    if (Serial3.available()) {
+    Serial3.write((char)Serial3.read());
   }
 }
\ No newline at end of file
diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp
index 376fe4a..24dda57 100644
--- a/src/sensors/data_source_camera_conicmirror.cpp
+++ b/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;
-  to_32u4 |= (CURRENT_DATA_READ.ySeen);
-  to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
-  BALL_32U4.write(to_32u4);
+  // byte to_32u4 = 0;
+  // to_32u4 |= (CURRENT_DATA_READ.ySeen);
+  // to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
+  // BALL_32U4.write(to_32u4);
 }
 
 void DataSourceCameraConic::test(){
diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp
index 2e40c37..b6c962e 100644
--- a/src/sensors/sensors.cpp
+++ b/src/sensors/sensors.cpp
@@ -2,35 +2,27 @@
 #include "sensors/sensors.h"
 
 void initSensors(){
-    pinMode(SWITCH_1, INPUT);
-    pinMode(SWITCH_2, INPUT);
+    pinMode(SWITCH_SX, 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));
-    // tone(BUZZER, 270, 250);
-    // delay(350);
+
+    // dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) };
+
+    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);
-    // delay(350);
-    ball = new DataSourceBall(&BALL_32U4, 57600);
-    // tone(BUZZER, 280, 250);
-    // 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);
+    ball = new DataSourceBall(&Serial4, 57600);
+    camera = new DataSourceCameraConic(&Serial2, 19200);
+    // usCtrl = new DataSourceCtrl(dUs);
+    bt = new DataSourceBT(&Serial3, 115200);
  }
 
 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();
 }
\ No newline at end of file