Login▼/회원가입BASIC4MCU | 질문게시판 | 아두이노 및 전기배선 작동이 안되어 해결 부탁드립니다
페이지 정보
작성자 호떡아 작성일2024-09-14 00:51 조회197회 댓글0건
https://www.basic4mcu.com/bbs/board.php?bo_table=gac&wr_id=23869
첨부파일
본문
#include <Servo.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <Adafruit_NeoPixel.h> // Neopixel을 사용하기 위해서 라이브러리를 불러옵니다.
#include <avr/power.h>
#define PIN 2
#define NUM_LEDS 23 // LED 개수
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRB + NEO_KHZ800);
ros::NodeHandle nh;
std_msgs::String str_msg;
ros::Publisher thrust("thrust", &str_msg);
Servo thrusterLeft;
Servo thrusterRight;
Servo thrusterCenter;
const int relayPin2 = 3;
const int relayPin = 4;
const int rcChannelPin1 = 5;
const int rcChannelPin2 = 6;
const int receiverPin = 7;
const int emergencyPin = 8;
const int thrustPinLeft = 9;
const int thrustPinRight = 10;
const int thrustPinCenter = 11;
const int relayPin3 = 12;
// 쓰러스터 보정 값 (필요에 따라 조정)
const int leftThrusterCorrection = 0;
const int rightThrusterCorrection = 0;
bool isAutonomousMode = false;
void leftThrusterCallback(const std_msgs::Int32& msg) {
if (isAutonomousMode) {
thrusterLeft.writeMicroseconds(msg.data);
}
}
void rightThrusterCallback(const std_msgs::Int32& msg) {
if (isAutonomousMode) {
thrusterRight.writeMicroseconds(msg.data);
}
}
void centerThrusterCallback(const std_msgs::Int32& msg) {
if (isAutonomousMode) {
thrusterCenter.writeMicroseconds(msg.data);
}
}
ros::Subscriber<std_msgs::Int32> sub_left("Thruster_left_topic", &leftThrusterCallback);
ros::Subscriber<std_msgs::Int32> sub_right("Thruster_right_topic", &rightThrusterCallback);
ros::Subscriber<std_msgs::Int32> sub_center("Thruster_center_topic", ¢erThrusterCallback);
void setup() {
strip.begin();
strip.show();
thrusterLeft.attach(thrustPinLeft);
thrusterRight.attach(thrustPinRight);
thrusterCenter.attach(thrustPinCenter);
pinMode(rcChannelPin1, INPUT);
pinMode(rcChannelPin2, INPUT);
pinMode(receiverPin, INPUT);
pinMode(emergencyPin, INPUT);
pinMode(relayPin, OUTPUT);
pinMode(relayPin2, OUTPUT);
pinMode(relayPin3, OUTPUT);
digitalWrite(relayPin, HIGH);
digitalWrite(relayPin2, HIGH);
digitalWrite(relayPin3, HIGH);
nh.initNode();
nh.advertise(thrust);
nh.subscribe(sub_left);
nh.subscribe(sub_right);
nh.subscribe(sub_center);
}
void loop() {
int rcValue1 = pulseIn(rcChannelPin1, HIGH);
int rcValue2 = pulseIn(rcChannelPin2, HIGH);
int rcValue3 = pulseIn(receiverPin, HIGH);
int rcValue4 = pulseIn(emergencyPin, HIGH);
int throttlePWM1 = map(rcValue1, 1100, 1900, 1100, 1900);
int throttlePWM2 = map(rcValue2, 1100, 1900, -380, 380);
int throttlePWM3 = map(rcValue3, 1100, 1900, 1100, 1900);
int throttlePWM4 = map(rcValue4, 1100, 1900, 1100, 1900);
const char* mode;
if (throttlePWM4 > 1600) { // 비상정지모드
for (int i = 0; i < NUM_LEDS; i++) {
strip.setPixelColor(i, 255, 0, 0); // R=255, G=0, B=0 (빨간색)
}
strip.show();
digitalWrite(relayPin, LOW);
digitalWrite(relayPin2, LOW);
digitalWrite(relayPin3, LOW);
isAutonomousMode = false;
mode = "EMERGENCY_STOP";
} else if (throttlePWM4 < 1400 && throttlePWM3 > 1600) { // RC모드
for (int i = 0; i < NUM_LEDS; i++) {
strip.setPixelColor(i, 0, 255, 0); // R=0, G=255, B=0 (초록색)
}
strip.show();
digitalWrite(relayPin, HIGH);
digitalWrite(relayPin2, HIGH);
digitalWrite(relayPin3, HIGH);
isAutonomousMode = false;
mode = "RC_MODE";
int combinedThrottleLeft = throttlePWM1 - throttlePWM2 + leftThrusterCorrection;
int combinedThrottleRight = throttlePWM1 + throttlePWM2 + rightThrusterCorrection;
int combinedThrottleCenter = throttlePWM1;
if (throttlePWM2 == 0) {
combinedThrottleLeft = throttlePWM1 + leftThrusterCorrection;
combinedThrottleRight = throttlePWM1 + rightThrusterCorrection;
}
if (throttlePWM1 >= 1401 && throttlePWM1 <= 1599) {
combinedThrottleLeft = 1500;
combinedThrottleRight = 1500;
combinedThrottleCenter = 1500;
}
combinedThrottleLeft = constrain(combinedThrottleLeft, 1100, 1900);
combinedThrottleRight = constrain(combinedThrottleRight, 1100, 1900);
combinedThrottleCenter = constrain(combinedThrottleCenter, 1100, 1900);
thrusterLeft.writeMicroseconds(combinedThrottleLeft);
thrusterRight.writeMicroseconds(combinedThrottleRight);
thrusterCenter.writeMicroseconds(combinedThrottleCenter);
char result_str[50];
snprintf(result_str, 100, "PWM1: %d, PWM2: %d, PWM3: %d, MODE: %s", combinedThrottleLeft, combinedThrottleRight, combinedThrottleCenter, mode);
str_msg.data = result_str;
thrust.publish(&str_msg);
} else if (throttlePWM4 < 1400 && throttlePWM3 > 1400 && throttlePWM3 <= 1600) { // 자율운항모드
for (int i = 0; i < NUM_LEDS; i++) {
strip.setPixelColor(i, 255, 255, 0); // R=255, G=255, B=0 (노란색)
}
strip.show();
delay(100);
for (int i = 0; i < NUM_LEDS; i++) {
strip.setPixelColor(i, 0, 0, 0); // 모든 LED 끄기
}
strip.show();
delay(100);
digitalWrite(relayPin, HIGH);
digitalWrite(relayPin2, HIGH);
digitalWrite(relayPin3, HIGH);
isAutonomousMode = true;
char result_str[50];
snprintf(result_str, 100, "MODE: AUTO_MODE");
str_msg.data = result_str;
thrust.publish(&str_msg);
} else if (throttlePWM4 < 1400 && throttlePWM3 <= 1400) { // 쓰러스터 제어
for (int i = 0; i < NUM_LEDS; i++) {
strip.setPixelColor(i, 0, 0, 0); // 모든 LED 끄기
}
strip.show();
isAutonomousMode = false;
delay(100);
digitalWrite(relayPin, HIGH);
digitalWrite(relayPin2, LOW);
digitalWrite(relayPin3, HIGH);
}
nh.spinOnce();
delay(20);
}
위 코드와 업로드된 회로도로 쓰러스터를 작동하려고 하는데 비상정지 모드만 작동이 되고 RC 및 자율운항모드는 작동이 되지 않습니다 LED또한 들어오지 않아서 답변 부탁드리겠습니다
- BASIC4MCU 작성글 SNS에 공유하기
댓글 0
조회수 197등록된 댓글이 없습니다.