The code of my Arduino project, Turret Car.

turret_car.ino

/*
 * Author: Grover Chou <groverchout@gmail.com>
 *
 * TIPS:
 * For enable debug, you can easily connect pin 2 to Vcc.
 * For force enable debug, set DEBUG_FORCE to true.
 * For debug bluetooth, connect the pin right the reset button to Vcc.
 * The baud rate for debug bluetooth is 38400, normal mode is 9600.
 */

#include <CmdMessenger.h>
#include <Servo.h>
#include <Wire.h>

// Debug, if this pin set to high, then enable debug mode.
#define DEBUG 2

// Debug enable without check
#define DEBUG_FORCE false

// Car wheel
#define CAR_WHEEL_LEFT    11
#define CAR_WHEEL_LEFT_1  10
#define CAR_WHEEL_LEFT_2  9
#define CAR_WHEEL_RIGHT   6
#define CAR_WHEEL_RIGHT_1 8
#define CAR_WHEEL_RIGHT_2 7

#define CAR_WHEEL_SPEED_BIAS 35

// Turret
#define TURRET        12
#define TURRET_STEER  5

#define TURRET_STEER_DEFALUT_POSITION 90

CmdMessenger cmdMessenger = CmdMessenger(Serial);

Servo turretSteer;

bool debug = false;

int turretSteerPosition = TURRET_STEER_DEFALUT_POSITION;

/*
 * Command defines
 * kWheel: Move or turn car wheel
 *         kWheel, <ActionNo>, <Speed>
 * kTurret: Turn turret
 *          kTurret, <ActionNo>
 * kFire: Fire the turret
 */
enum {
  kWheel = 0,
  kTurret = 1,
  kFire = 2
};

void attachCommandCallbacks() {
  cmdMessenger.attach(onUnknownCommandReceived);
  cmdMessenger.attach(kWheel, onWheelActionReceived);
  cmdMessenger.attach(kTurret, onTurretActionReceived);
  cmdMessenger.attach(kFire, onFireActionReceived);
}

void onUnknownCommandReceived() {
  if (debug) {
    Serial.println("Unknown command received");
  }
}

void onWheelActionReceived() {
  int wheelAction = cmdMessenger.readInt16Arg();
  int wheelSpeed = cmdMessenger.readInt16Arg();

  int var = map(wheelSpeed, 1, 100, 128, 255);

  if (debug) {
    Serial.println("Wheel action received");
    Serial.print("ActionNo: ");
    Serial.print(wheelAction);
    Serial.print(" Speed: ");
    Serial.println(wheelSpeed);
  }

  if (wheelSpeed != 0) {
    switch (wheelAction) {
      case 1:
        // Right
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, HIGH);
        digitalWrite(CAR_WHEEL_LEFT_2, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_1, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_2, LOW);
        break;
      case 2:
        // Right - Forward
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var - CAR_WHEEL_SPEED_BIAS * 2);
        digitalWrite(CAR_WHEEL_LEFT_1, HIGH);
        digitalWrite(CAR_WHEEL_LEFT_2, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_1, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_2, HIGH);
        break;
      case 3:
        // Forward
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, HIGH);
        digitalWrite(CAR_WHEEL_LEFT_2, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_1, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_2, HIGH);
        break;
      case 4:
        // Left - Forward
        analogWrite(CAR_WHEEL_LEFT, var - CAR_WHEEL_SPEED_BIAS);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, HIGH);
        digitalWrite(CAR_WHEEL_LEFT_2, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_1, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_2, HIGH);
        break;
      case 5:
        // Left
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, LOW);
        digitalWrite(CAR_WHEEL_LEFT_2, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_1, LOW);
        digitalWrite(CAR_WHEEL_RIGHT_2, HIGH);
        break;
      case 6:
        // Left - Backward
        analogWrite(CAR_WHEEL_LEFT, var - CAR_WHEEL_SPEED_BIAS);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, LOW);
        digitalWrite(CAR_WHEEL_LEFT_2, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_1, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_2, LOW);
        break;
      case 7:
        // Backward
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var);
        digitalWrite(CAR_WHEEL_LEFT_1, LOW);
        digitalWrite(CAR_WHEEL_LEFT_2, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_1, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_2, LOW);
        break;
      case 8:
        // Right - Backward
        analogWrite(CAR_WHEEL_LEFT, var);
        analogWrite(CAR_WHEEL_RIGHT, var - CAR_WHEEL_SPEED_BIAS * 2);
        digitalWrite(CAR_WHEEL_LEFT_1, LOW);
        digitalWrite(CAR_WHEEL_LEFT_2, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_1, HIGH);
        digitalWrite(CAR_WHEEL_RIGHT_2, LOW);
        break;
      default:
        break;
    }
    delay(75);
  }
  analogWrite(CAR_WHEEL_LEFT, 0);
  analogWrite(CAR_WHEEL_RIGHT, 0);
}

void onTurretActionReceived() {
  int turretAction = cmdMessenger.readInt16Arg();
  int turretSpeed = cmdMessenger.readInt16Arg();

  int turretPositionGain = map(turretSpeed, 1, 100, 1, 20);

  if (debug) {
    Serial.println("Turret action received");
    Serial.print("ActionNo: ");
    Serial.print(turretAction);
    Serial.print(" Speed: ");
    Serial.println(turretSpeed);
  }

  if (turretAction == 1) {
    // Right
    int var = turretSteerPosition - turretPositionGain;
    if (var < 0) {
      turretSteerPosition = 0;
    } else {
      turretSteerPosition = var;
    }
    turretSteer.write(turretSteerPosition);
  } else if (turretAction == 2) {
    // Left
    int var = turretSteerPosition + turretPositionGain;
    if (var > 180) {
      turretSteerPosition = 180;
    } else {
      turretSteerPosition = var;
    }
    turretSteer.write(turretSteerPosition);
  }
}

void onFireActionReceived() {
  if (debug) {
    Serial.println("Fire action received");
  }

  digitalWrite(TURRET, LOW);
  delay(175);
  digitalWrite(TURRET, HIGH);
}

void printDebugInfo() {
  Serial.println("Welcome to turret car debug channel");
  Serial.println("Bluetooth name: Turret Car");
  Serial.println("Bluetooth password: 0130");
}

void setup() {
  pinMode(DEBUG, OUTPUT);
  if (DEBUG_FORCE || digitalRead(DEBUG)) {
    debug = true;
    Serial.begin(9600);
    printDebugInfo();
  }

  pinMode(CAR_WHEEL_LEFT, OUTPUT);
  pinMode(CAR_WHEEL_RIGHT, OUTPUT);

  turretSteer.attach(TURRET_STEER, 650, 2450);
  turretSteer.write(turretSteerPosition);

  pinMode(TURRET, OUTPUT);
  digitalWrite(TURRET, HIGH);

  attachCommandCallbacks();

  if (debug) {
    Serial.println("System ready");
    Serial.println("-----------------------------------");
  }
}

void loop() {
  cmdMessenger.feedinSerialData();
}