     return nodeAdress[wheel];
 }
 
-void robot_initNode(uint8_t wheel)
-{
-    robot_velocityMode(wheel);
-    robot_motorPowerOn(wheel);
-    robot_motorStart(wheel);
-    robot_setMotorSpeed(wheel, 0);
-}
-
 void robot_velocityMode(uint8_t wheel)
 {
-    #if DEBUG
+    #if DEBUGMODE
     Serial.println("# Motor velocity mode");
     #endif
 

    CAN.send_0();
    delay(10);

    #if DEBUG
    for (int i = 0; i < 8; i++)
    {
        Serial.print(data[i], HEX);


void robot_motorStart(uint8_t wheel)
{
    #if DEBUGMODE
    Serial.println("# Motor start");
    #endif

