diff --git a/src/main.cpp b/src/main.cpp
index 63bd3df..db7efc8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -19,10 +19,13 @@ MoveControl moveController;
 
 uint64_t last_millis = 0;
 int controller_battery = -1;
-uint8_t controller_led = 1;
 
 // Temp code
 double speed = 0;
+uint8_t drive_mode = 0;
+void driveWithControllerJoystick(int8_t x, int8_t y);
+void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right);
+void changeDriveMode();
 
 void setup() {
   Serial.begin(115200);      // make sure your Serial Monitor is also set at this baud rate.
@@ -56,23 +59,31 @@ void loop() {
   right_motor.runMotorControl();
   speedometer_left.runSpeedometer();
   speedometer_right.runSpeedometer();
-  moveController.runMoveControl();
+
+  if (drive_mode == 1)
+    moveController.runMoveControl();
 }
 
 void callbackControllerAction() {
   if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
   if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
   if (Ps3.event.button_down.r1) { moveController.setSpeed(speed += 0.1); }
-  if (Ps3.event.button_down.up) { moveController.setDrivingStatus(drivingStatus::straightForward); }
-  if (Ps3.event.button_up.up)   { moveController.setDrivingStatus(drivingStatus::stop); }
+  if (Ps3.event.button_down.start) { changeDriveMode(); }
+  if (abs(Ps3.event.analog_changed.stick.lx) || abs(Ps3.event.analog_changed.stick.ly))
+    driveWithControllerJoystick(Ps3.data.analog.stick.lx, Ps3.data.analog.stick.ly);
+
+  if (abs(Ps3.event.analog_changed.button.l2) || abs(Ps3.event.analog_changed.button.r2)) {
+    driveWithControllerShoulderTrigger(Ps3.data.analog.button.l2, Ps3.data.analog.button.r2);
+    Serial.println(Ps3.data.analog.button.l2, DEC);
+  }
 }
 
 void callbackControllerConnect() {
   Serial.println("Controller connected to ESP32");
 
   delay(400);
-  Serial.print("Setting LEDs to Status "); Serial.println(controller_led, DEC);
-  Ps3.setPlayer(controller_led);
+  Serial.print("Setting LEDs to Status "); Serial.println(drive_mode, DEC);
+  Ps3.setPlayer(drive_mode);
 }
 
 void callbackControllerDisconnect() {
@@ -93,3 +104,31 @@ void controllerPrintBattery() {
   else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
   else Serial.println("UNDEFINED");
 }
+
+void driveWithControllerJoystick(int8_t x, int8_t y) {
+  if (drive_mode != 1) return;
+
+  
+}
+
+void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) {
+  if (drive_mode != 2) return;
+
+  map(left, 0, 255, 0, 100);
+  map(right, 0, 255, 0, 100);
+
+  left_motor.setTargetPower(left);
+  right_motor.setTargetPower(right);
+}
+
+void changeDriveMode() {
+  drive_mode++;
+  if (drive_mode > 2) {
+    drive_mode = 1;
+  }
+  Ps3.setPlayer(drive_mode);
+
+  if (drive_mode == 2) {
+    moveController.setSpeed(0);
+  }
+}
\ No newline at end of file