현재 온도센서 3개, 서보모터 2개, 수위센서 1개, 워터펌프모터 1개를 사용하여 작은 소방차(?)를 만들어 보려고 하는데 아래와 같은

코드를 완성을 했습니다. 그러나 워터펌프모터가 동작을 하지 않고, 수위센서와 관련된 LED가 연동이 되지를 않습니다. 어느 방향으로

코드를 수정해야 하는지 조언을 듣고 싶습니다.

---------------------------------------------------------------------------------------------------------

#define FORWARD   1
#define BACKWARD  0

#define SPEED_100  0xff
#define SPEED_80   0xC0
#define SPEED_50   0x80
#define SPEED_30   0x40
#define SPEED_0    0x00

#define    B_FORWARD_L        '1'
#define B_FORWARD               '2'
#define    B_FORWARD_R        '3'
#define B_CCLOCKWISE           '4'
#define    B_STOP                    '5'
#define B_CLOCKWISE             '6'
#define    Servo_L                      '7'
#define    B_BACKWARD          '8'
#define    Servo_R                      '9'

void forw();
void forw_l();
void forw_r();
void back();
void servo_l();
void servo_r();
void clockwise();
void cclockwise();
void sstop();

int motor_a_dir = 4;
int motor_a_pwm = 5;
int motor_b_dir = 7;
int motor_b_pwm = 6;

int temp1Pin = A0; //온도센서1
int temp2Pin = A2; //온도센서2
int temp3Pin = A4; //온도센서3
float tempC1;
float tempC2;
float tempC3;

int WPPin = 10; //워터펌프모터

int WLSPin = A5; //수위센서
float WLS;

int LEDPin = 13; //LED

#include <Servo.h>
Servo myservo1;
Servo myservo2;
int angle = 0;

void setup() {
  Serial.begin(115200);
 
  myservo1.attach(11); //서보모터1
  myservo2.attach(12); //서보모터2
  pinMode(motor_a_dir, OUTPUT);
  pinMode(motor_a_pwm, OUTPUT);
  pinMode(motor_b_dir, OUTPUT);
  pinMode(motor_b_pwm, OUTPUT);
}

void loop()
{
  char rx_data;  
  if (Serial.available() > 0)
  {
    rx_data = Serial.read();
    if (rx_data == B_FORWARD)          forw();
    else if (rx_data == B_FORWARD_L)   forw_l();
    else if (rx_data == B_FORWARD_R)   forw_r();
    else if (rx_data == B_BACKWARD)    back();
    else if (rx_data == Servo_L)  servo_l();
    else if (rx_data == Servo_R)  servo_r();
    else if (rx_data == B_CLOCKWISE)   clockwise();
    else if (rx_data == B_CCLOCKWISE)  cclockwise();
    else if (rx_data == B_STOP)        sstop();
    else    ;
  }
  delay(100);
 
  WLS = analogRead(WLSPin);
  tempC1 = analogRead(temp1Pin);
  tempC1 = map(tempC1, 0, 1023, 0, 500);
  tempC2 = analogRead(temp2Pin);
  tempC2 = map(tempC2, 0, 1023, 0, 500);
  tempC3 = analogRead(temp3Pin);
  tempC3 = map(tempC3, 0, 1023, 0, 500);
 
  WLS = analogRead(WLSPin);

 if(tempC1 >= 40 && tempC1 > tempC2 && tempC1 > tempC3){
      myservo1.write(0);
      digitalWrite(WPPin, HIGH);
  }
  else if(tempC2 >= 40 && tempC2 > tempC1 && tempC2 > tempC3){
          myservo1.write(90);
          digitalWrite(WPPin, HIGH);
  }
  else if(tempC3 >= 40 && tempC3 > tempC1 && tempC3 > tempC2){
          myservo1.write(180);
          digitalWrite(WPPin, HIGH);
  }
  else{
    myservo1.write(90);
    digitalWrite(WPPin, LOW);
  }
 
   if(WLS >= 600)
  {
    digitalWrite(LEDPin, HIGH);
  }
  else if(WLS < 600)
  {
    digitalWrite(LEDPin, LOW);
  }

  Serial.println("temp1 = ");
  Serial.println(tempC1);
  Serial.println("temp2 = ");
  Serial.println(tempC2);
  Serial.println("temp3 = ");
  Serial.println(tempC3);
  Serial.println("WLS = ");
  Serial.println(WLS);
 
  delay(500);
}

void forw()
{
  digitalWrite(motor_a_dir, FORWARD); analogWrite(motor_a_pwm, SPEED_80);
  digitalWrite(motor_b_dir, FORWARD); analogWrite(motor_b_pwm, SPEED_80);
}

void forw_l()
{
  digitalWrite(motor_a_dir, FORWARD); analogWrite(motor_a_pwm, SPEED_50);
  digitalWrite(motor_b_dir, FORWARD); analogWrite(motor_b_pwm, SPEED_80);
}

void forw_r()
{
  digitalWrite(motor_a_dir, FORWARD); analogWrite(motor_a_pwm, SPEED_80);
  digitalWrite(motor_b_dir, FORWARD); analogWrite(motor_b_pwm, SPEED_50);
}

void back()
{
  digitalWrite(motor_a_dir, BACKWARD); analogWrite(motor_a_pwm, SPEED_80);
  digitalWrite(motor_b_dir, BACKWARD); analogWrite(motor_b_pwm, SPEED_80);
}

void servo_l()
{
  myservo2.write(180);
}

void servo_r()
{
  myservo2.write(0);
}

void clockwise()
{
  digitalWrite(motor_a_dir, BACKWARD); analogWrite(motor_a_pwm, SPEED_80);
  digitalWrite(motor_b_dir, FORWARD); analogWrite(motor_b_pwm, SPEED_80);
}

void cclockwise()
{
  digitalWrite(motor_a_dir, FORWARD); analogWrite(motor_a_pwm, SPEED_80);
  digitalWrite(motor_b_dir, BACKWARD); analogWrite(motor_b_pwm, SPEED_80);
}

void sstop()
{
  digitalWrite(motor_a_dir, FORWARD); analogWrite(motor_a_pwm, SPEED_0);
  digitalWrite(motor_b_dir, FORWARD); analogWrite(motor_b_pwm, SPEED_0);
}