WW2 Tank Laser Tag Sherman & Panther

import car

bt_port = "/dev/rfcomm3"
laser_pin = 20
rc_car = car.Car(bt_port, laser_pin)

rc_car.configMotors(6,5,19,13, invertLR=True)
rc_car.configServos() #Turret drive servo, barrel servo

while 1:
    rc_car.update()
        

#include "Timer.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

int JS_VALS[2] = {};
int JS_VALS_R[2] = {};

const int JS_PINS[4] = {A0, A1, A2, A3}; //L_X, L_Y, R_X, R_Y
const int led_pins[4] = {4,5,6,7};

const int button_pin = 3;
const int deadzone = 8;
bool laser = false;

const int servo_limits[5] = {0, 180, 50, 95, 75}; //Left, Right, Lower, Upper, Middle limits for the turret
int turret_positions[2] = {90, servo_limits[4]}; //Initial positions

unsigned long last_time = 0;
long motor_delay = 30; //Delay 30 ms between motor control polls
long turret_delay = 150; //Delay 150 ms between turret control polls

long reloadSpeed = 8000;
float driveSpeed = 1.0;
int crewValues[4] = {3,3,2,1};
bool crewLeft[4] = {true,true,true,true};
int crewLeftNum = 4;
int reloadTimer, crewReplaceTimer, fireTimer;
bool is_reloaded= true;
bool can_move = true;
bool can_fire = true;

LiquidCrystal_I2C lcd(0x3F, 16, 2);

Timer t;

void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
lcd.init();
lcd.backlight();
lcd.print("Begin fighting!");
delay(2000);
lcd.clear();
lcd.home();
pinMode(button_pin, INPUT_PULLUP);
Serial.println("Begin");

t.every(motor_delay, control_motors);
t.every(turret_delay, control_turret);
t.every(50, checkButton);
for(int i=0;i<4;i++){
    pinMode(led_pins[i], OUTPUT);
}
updateCrew();

}

void loop() {
  /*control_motors();
  control_turret();
  delay(150);*/
  t.update();
  if(Serial.available()>0){
      String data = Serial.readStringUntil('\n');
      if(data=="HG"){
          crewLeft[0] = false;
          crewLeftNum -= 1;
          reloadSpeed += 2000;
          can_fire = false;
          crewReplaceTimer = t.after(6000, replaceCrew);
      }
      else if(data=="HD"){
          crewLeft[1] = false;
          crewLeftNum -= 1;
          driveSpeed -= .2;
          can_move = false;
          crewReplaceTimer = t.after(6000, replaceCrew);
      }
      else if(data=="HC"){
          crewLeft[2] = false;
          crewLeftNum -= 1;
          driveSpeed -= .1;
          reloadSpeed += 1500;
          crewReplaceTimer = t.after(6000, replaceCrew);
      }
      else if(data=="HM"){
          crewLeft[3] = false;
          crewLeftNum -= 1;
          crewReplaceTimer = t.after(6000, replaceCrew);
      }
      updateCrew();
  }
  if(crewLeftNum<=1){
      lcd_clear();
      lcd.print("You lose");
      while(1);
  }
}

void checkButton(){
    if(digitalRead(button_pin)==0){
        fire_laser();
    }
}

void replaceCrew(){
    if(crewLeft[0]==false){
        if(crewLeft[2]==true){
            crewLeft[2] = false;
            crewLeft[0] = true;
            can_fire = true;
        }
        else if(crewLeft[3]==true){
            crewLeft[3] = false;
            crewLeft[0] = true;
            can_fire = true;
        }
    }
    else if(crewLeft[1]==false){
        if(crewLeft[2]==true){
            crewLeft[2] = false;
            crewLeft[1] = true;
            can_move = true;
        }
        else if(crewLeft[3]==true){
            crewLeft[3] = false;
            crewLeft[1] = true;
            can_move = true;
        }
    }
    updateCrew();
    t.stop(crewReplaceTimer);
}

void updateCrew(){
    for(int i=0;i<4;i++){
        digitalWrite(led_pins[i],crewLeft[i]);
    }
}

void lcd_clear(){
    lcd.clear();
    lcd.home();
}

void fire_laser(){
  if(is_reloaded){
      if(can_fire){
        lcd.clear();
        lcd.print("Firing laser");
        is_reloaded = false;
        String json_string = "{\"motor\":[" + String(0)+","+String(0)+"]}"; //Stop tank before firing
        Serial.println(json_string);
        laser = true;
        fireTimer = t.after(2000, stopLaser);
        
      }
    }
}

void stopLaser(){
    t.stop(fireTimer);
    laser = false;
    lcd_clear();
    lcd.print("Reloading");
    reloadTimer = t.after(reloadSpeed, reloadGun);
}

void reloadGun(){
    lcd_clear();
    lcd.print("Reloaded");
    is_reloaded = true;
    t.stop(reloadTimer);
}

void control_motors(){
  JS_VALS[0] = analogRead(JS_PINS[0]);
  JS_VALS[1] = analogRead(JS_PINS[1]);
  int x = map(JS_VALS[0], 0, 1023, -50, 50);
  x = constrain(x, -100, 100);
  int y = map(JS_VALS[1], 0, 1023, -50, 50);
  y = constrain(y, -100, 100);
  int motor1 = x+y;
  int motor2 = y-x;
  motor1 = constrain(motor1, -100, 100);
  motor2 = constrain(motor2, -100, 100);
  if(abs(motor1) <= deadzone){
    motor1 = 0;
  }
  if(abs(motor2) <= deadzone){
    motor2 = 0;
  }
  
  motor1 = int(float(motor1)*driveSpeed);
  motor2 = int(float(motor2)*driveSpeed);
  
  if(can_move==false){
      motor1 = 0;
      motor2 = 0;
  }
  
  String json_string = "{\"motor\":[" + String(motor1)+","+String(motor2)+"]}";
  Serial.println(json_string);
  String json_string2;
  if(laser){
    json_string2 = "{\"shoot\":1}";
    Serial.println(json_string2);
  }
  else if(!laser){
    json_string2 = "{\"shoot\":0}";
    Serial.println(json_string2);
  }
}

void control_turret(){
  JS_VALS_R[0] = analogRead(JS_PINS[2]); //Get X values
  JS_VALS_R[1] = analogRead(JS_PINS[3]); //Get Y values
  int x = map(JS_VALS_R[0]+10, 0, 1023, 5, -5);
  x = constrain(x, -5, 5); //Catch outliers
  int y = map(JS_VALS_R[1], 0, 1023, 2, -2);
  y = constrain(y, -2, 2);

  turret_positions[0] += x;
  turret_positions[1] += y;

  if(turret_positions[0] <= servo_limits[0]){
    turret_positions[0] = servo_limits[0];
  }
  else if(turret_positions[0] >= servo_limits[1]){
    turret_positions[0] = servo_limits[1];
  }
  if(turret_positions[1] <= servo_limits[2]){
    turret_positions[1] = servo_limits[2];
  }
  else if(turret_positions[1] >= servo_limits[3]){
    turret_positions[1] = servo_limits[3];
  }
  
  String json_string = "{\"turret\":[" + String(turret_positions[0])+","+String(turret_positions[1])+"]}";
  Serial.println(json_string);
  
}