 }
 
 void robotAction() {
-  int leftSpeed = robotSpeed;
-  int rightSpeed = robotSpeed;
   switch (state) {
     case State::Forward:
     case State::Backward:
       // keep it at robotSpeed
+      Rp6.moveAtSpeed(robotSpeed, robotSpeed);
       break;
     case State::Left:
-      leftSpeed *= invertedRotateSpeed;
+      //Rp6.rotate(robotSpeed, RP6_LEFT, 10); // 10 degrees, will this work right?
+      Rp6.moveAtSpeed(robotSpeed * invertedRotateSpeed, robotSpeed);
       break;
     case State::Right:
-      rightSpeed *= invertedRotateSpeed;
+      //Rp6.rotate(robotSpeed, RP6_RIGHT, 10); // same as above
+      Rp6.moveAtSpeed(robotSpeed, robotSpeed * invertedRotateSpeed);
       break;
     default:
-      rightSpeed *= 0;
-      leftSpeed *= 0;
-  }
-  Rp6.moveAtSpeed(leftSpeed, rightSpeed);
+      Rp6.moveAtSpeed(0, 0);
+      break;
+  }
 }
 
 void connection() {

    if (paramcheck > 0) {
      useCommandWithParams(command);
    } else {
      Serial.println("NACK");
    }
  }
}

#include <SoftwareSerial.h>
#include <Rp6.h>

#define DEBUG 1

enum class State { Forward, Backward, Left, Right, Startup };
State state = State::Startup;
SoftwareSerial Comport(D7, D8);

int robotSpeed = 0;
float invertedRotateSpeed = 0; // can be 0 to 1, the higher it is, the slower the robot rotates

// Create an instance of the server
// specify the port to listen on as an argument
WiFiServer server(80);
