         stop_movement();
         current_direction = 0;
         return;
+     } else if (current_direction == FORWARD_LEFT) {
+        go_forward();
      }
-     //right.setMotorDirection(MOTOR_FORWARD);
-     //right.setSpeed(MOTOR_SPEED);   
-     left.setMotorDirection(MOTOR_BACKWARD);
-     left.setSpeed(MOTOR_SPEED / 2);   
-     current_direction = RIGHT;
+     if (current_direction == FORWARD) {
+        right_motor(MOTOR_FORWARD, MOTOR_SPEED + 70);
+        left_motor(MOTOR_BACKWARDS, MOTOR_SPEED / 2);
+        current_direction = FORWARD_RIGHT;
+     } else {
+        right_motor(MOTOR_FORWARD, MOTOR_SPEED / 2);
+        left_motor(MOTOR_BACKWARDS, MOTOR_SPEED / 2);
+        current_direction = RIGHT;
+     }
 }
 
 void go_forward() {

    process_cmd();
  }

  delay(50); 
}

#define LEFT 1
#define RIGHT 2
#define FORWARD 3
#define BACKWARD 4

#define MOTOR_FORWARD 0
#define MOTOR_BACKWARD 1

#define MOTOR_SPEED 150

int current_direction = 0;



#include <Dagu4Motor.h>

                 // pwm, dir, cur(analog), encA, encB 
Dagu4Motor left_front(5, 4, 0, 0, 0);
Dagu4Motor left_back(3, 2, 0, 0, 0);
Dagu4Motor right_front(11, 12, 0, 0, 0);
Dagu4Motor right_back(9, 8, 0, 0, 0);

void setup() {
  //Serial.begin(9600);
