diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini
index 74e0b85..a759816 100644
--- a/controller/fw/embed/platformio.ini
+++ b/controller/fw/embed/platformio.ini
@@ -16,7 +16,7 @@ board = genericSTM32F446RE
 framework = arduino
 upload_protocol = stlink
 debug_tool = stlink
-monitor_speed = 115200
+monitor_speed = 19200
 monitor_parity = N
 build_flags = 
 	-DSTM32F446xx
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 05567b7..36d46d5 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -31,17 +31,17 @@ LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
 Commander command(Serial);
 
 struct MotorControlInputs {
-  float target_angle;
-  float target_velocity;
-  bool motor_enabled;
+  float target_angle = 0.0;
+  float target_velocity = 0.0;
+  bool motor_enabled = false;
 };
 
 MotorControlInputs motor_control_inputs;
 
 void doMotor(char *cmd) {
   command.motor(&motor, cmd);
-  // digitalWrite(PC10, !digitalRead(PC10));
-  // delayMicroseconds(2);
+  digitalWrite(PC10, !digitalRead(PC10));
+  delayMicroseconds(2);
 }
 
 void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
@@ -57,14 +57,14 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
   current_sense->linkDriver(driver);
   current_sense->init();
   motor->linkCurrentSense(current_sense);
-  SimpleFOCDebug::enable(nullptr);
+  // SimpleFOCDebug::enable(nullptr);
   commander->add('M', callback, "motor");
   motor->useMonitoring(Serial);
-  motor->monitor_start_char = 'M';
-  motor->monitor_end_char = 'M';
+  // motor->monitor_start_char = 'M';
+  // motor->monitor_end_char = 'M';
   // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
   motor->monitor_downsample = 10; // default 10
-  commander->verbose = VerboseMode::user_friendly;
+  // commander->verbose = VerboseMode::user_friendly;
   motor->controller = MotionControlType::angle;
   motor->torque_controller = TorqueControlType::voltage;
   motor->foc_modulation = FOCModulationType::SinePWM;
@@ -75,11 +75,13 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
   motor->LPF_velocity.Tf = 0.005;
   motor->LPF_angle.Tf = 0.005;
   motor->P_angle.P = 15;
+  // motor->P_angle.I = 15;
+  // motor->P_angle.3 = 15;
   motor->velocity_limit = 40;
   motor->voltage_limit = 40;
   motor->current_limit = 0.5;
   motor->sensor_direction = Direction::CCW;
-  current_sense->skip_align = true;
+  current_sense->skip_align = false;
   motor->init();
   motor->initFOC();
 }
@@ -166,7 +168,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
 void setup() {
   Serial.setRx(HARDWARE_SERIAL_RX_PIN);
   Serial.setTx(HARDWARE_SERIAL_TX_PIN);
-  Serial.begin(115200);
+  Serial.begin(19200);
   // setup led pin
   pinMode(PC11, OUTPUT);
   pinMode(PC10, OUTPUT);
@@ -180,10 +182,10 @@ void setup() {
   SendTimer->attachInterrupt(send_data);
   SendTimer->resume();
   setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
-  _delay(1000);
+  // _delay(1000);
 }
 
-float current_angle = 0.0;
+// float current_angle = 0.0;
 void loop() {
   foc_step(&motor, &command);
   // current_angle = encoder.getAngle();