정보나눔

오픈소스하드웨어 프로젝트에 대한 다양한 정보를 나누는 공간입니다.

죄송하지만 코드 관련하여 여쭤보고 싶습니다.
박진서 | 2015-06-05

 현재 온도센서 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);
}

프로필사진

박진서 2015-06-05 20:49:17

해결했습니다.

너무 loop에만 신경을 써서 setup쪽에 누락된 부분이 있을거라 생각을 못했습니다.

프로필사진

Klant 2015-06-08 11:16:58

해결하셨다니 다행이네요!^^

이전글   |    앱인벤터 관련 2015-06-05
다음글   |    조도센서 질문이요! 2015-06-05