RC tracked robot

Hi everyone, here are the 3D models and all the schematics and code of the tracked robot that I built, and that you can see in the video: 

If you need a Floppy Board you can buy it on my Ko-Fi page!

Copy and paste the code into the Arduino IDE, change the pins to suit your needs and you are ready to go!

/* definizione pin cambia i pin qui in base alle tue esigenze */
#define TH_PIN PA1
#define ST_PIN PA0
#define IN3_M_DX_PIN PA5
#define IN4_M_DX_PIN PA6
#define PWM_M_DX_PIN PA7
#define IN1_M_SX_PIN PA3
#define IN2_M_SX_PIN PA4
#define PWM_M_SX_PIN PB0

#define ONBOARD_LED_PIN PC15
#define BREADBOARD_LED_PIN PB7

/* impostazioni firmware */
#define THROTTLE_MAX 1990
#define THROTTLE_MIN 1010
#define STEERING_MAX 1990
#define STEERING_MIN 1010

#define MOTOR_RESOLUTION 10

int throttle = 0;
int steering = 0;

int throttlePrev = 0;
int steeringPrev = 0;

void setup() {
  Serial.begin(9600);
  pinMode(TH_PIN, INPUT);
  pinMode(ST_PIN, INPUT);
  pinMode(IN3_M_DX_PIN, OUTPUT);
  pinMode(IN4_M_DX_PIN, OUTPUT);
  pinMode(IN1_M_SX_PIN, OUTPUT);
  pinMode(IN2_M_SX_PIN, OUTPUT);
  pinMode(PWM_M_DX_PIN, OUTPUT);
  pinMode(PWM_M_SX_PIN, OUTPUT);

  digitalWrite(IN3_M_DX_PIN, LOW);
  digitalWrite(IN4_M_DX_PIN, LOW);
  digitalWrite(IN1_M_SX_PIN, LOW);
  digitalWrite(IN2_M_SX_PIN, LOW);
  digitalWrite(PWM_M_DX_PIN, LOW);
  digitalWrite(PWM_M_SX_PIN, LOW);

  pinMode(BREADBOARD_LED_PIN, OUTPUT);
  for (int i = 0; i < 10; i++) {
    digitalWrite(BREADBOARD_LED_PIN, HIGH);
    delay(250);
    digitalWrite(BREADBOARD_LED_PIN, LOW);
    delay(250);
  }

  throttle = pulseIn(TH_PIN, HIGH);
  steering = pulseIn(ST_PIN, HIGH);
  throttlePrev = throttle;
  steeringPrev = steering;
}



void loop() {
  readReciever();

  int speed = map(throttle, THROTTLE_MIN, THROTTLE_MAX, -MOTOR_RESOLUTION, MOTOR_RESOLUTION);
  int steer = map(steering, STEERING_MIN, STEERING_MAX, -MOTOR_RESOLUTION, MOTOR_RESOLUTION);

  controlMotors(speed, steer);


  Serial.print("STEER: ");
  Serial.print(steer);
  Serial.print("\t");
  Serial.print("SPEED: ");
  Serial.print(speed);
  Serial.print("\t");
  Serial.print("TH: ");
  Serial.print(throttle);
  Serial.print("\t");
  Serial.print("ST: ");
  Serial.print(steering);
  Serial.println();
}

void controlMotors(int speed, int steer) {
  int velDX = speed - steer;
  int velSX = speed + steer;


  if (velDX >= MOTOR_RESOLUTION) {
    velDX = MOTOR_RESOLUTION;
  } else if (velDX <= -MOTOR_RESOLUTION) {
    velDX = -MOTOR_RESOLUTION;
  }
  if (velSX >= MOTOR_RESOLUTION) {
    velSX = MOTOR_RESOLUTION;
  } else if (velSX <= -MOTOR_RESOLUTION) {
    velSX = -MOTOR_RESOLUTION;
  }

  setMotorVel('d', velDX);
  setMotorVel('s', velSX);
}

void setMotorVel(char motor, int velocity) {
  int v = map(abs(velocity), 0, MOTOR_RESOLUTION, 0, 255);
  if (velocity >= 0) {
    setMotorDir(motor, 'f');
  } else {
    setMotorDir(motor, 'b');
  }
  if (motor == 'd') {
    analogWrite(PWM_M_DX_PIN, v);
  }
  if (motor == 's') {
    analogWrite(PWM_M_SX_PIN, v);
  }
}
void setMotorDir(char motor, char direction) {
  /* impostazione motore destro */
  if (motor == 'd') {
    if (direction == 'f') {
      digitalWrite(IN3_M_DX_PIN, HIGH);
      delayMicroseconds(100);
      digitalWrite(IN4_M_DX_PIN, LOW);
    }
    if (direction == 'b') {
      digitalWrite(IN3_M_DX_PIN, LOW);
      delayMicroseconds(100);
      digitalWrite(IN4_M_DX_PIN, HIGH);
    }
  }
  /* impostazione direzione motore sinistro */
  if (motor == 's') {
    if (direction == 'f') {
      digitalWrite(IN1_M_SX_PIN, HIGH);
      delayMicroseconds(100);
      digitalWrite(IN2_M_SX_PIN, LOW);
    }
    if (direction == 'b') {
      digitalWrite(IN1_M_SX_PIN, LOW);
      delayMicroseconds(100);
      digitalWrite(IN2_M_SX_PIN, HIGH);
    }
  }
}

void readReciever() {
  throttle = pulseIn(TH_PIN, HIGH);
  steering = pulseIn(ST_PIN, HIGH);

  /* un piccolo filtro per rendere più stabile il valore letto dal ricevitore */
  throttle = (throttle * 0.3) + (throttlePrev * 0.70);
  steering = (steering * 0.3) + (steeringPrev * 0.70);
  throttlePrev = throttle;
  steeringPrev = steering;
  /*===========*/

  if (throttle >= THROTTLE_MAX) {
    throttle = THROTTLE_MAX;
  } else if (throttle <= THROTTLE_MIN) {
    throttle = THROTTLE_MIN;
  }

  if (steering >= STEERING_MAX) {
    steering = STEERING_MAX;
  } else if (steering <= STEERING_MIN) {
    steering = STEERING_MIN;
  }
}