diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index d022e94..49732ad 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -133,8 +133,7 @@ void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
   // Copy message ID 
   memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
   // Copy all data bytes
-  for(int i = 1;i < CAN_MSG_MAX_LEN;i++)
-    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(T));  //As byte variable
+  memcpy(crc_data + 1, msg_l.buf, 6);
   // Calculate CRC
   uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
  
@@ -306,7 +305,7 @@ void listen_can(const CAN_message_t &msg) {
           }
           break;
         case MOTOR_ANGLE:
-          memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
+          memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
                 sizeof(motor_control_inputs.target_angle));
           setup_angle(motor_control_inputs.target_angle);
           break;
diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py
index 03d0f40..90d95a6 100644
--- a/controller/fw/embed/test/python_send_angle.py
+++ b/controller/fw/embed/test/python_send_angle.py
@@ -62,6 +62,5 @@ def main():
     send_target_angle(bus, args.angle)
 
     bus.shutdown()
-
 if __name__ == '__main__':
     main()