BASIC4MCU | 질문게시판 | 답변 : 아두이노 및 전기배선 작동이 안되어 해결 부탁드립니다
페이지 정보
작성자 master 작성일2024-09-14 10:33 조회388회 댓글0건본문
#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,thrusterRight,thrusterCenter;
const int relayPin2=3,relayPin=4;
const int rcChannelPin1=5,rcChannelPin2=6;
const int receiverPin=7;
const int emergencyPin=8;
const int thrustPinLeft=9,thrustPinRight=10,thrustPinCenter=11;
const int relayPin3=12;
// 쓰러스터 보정 값(필요에 따라 조정)
const int leftThrusterCorrection=0,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,1); digitalWrite(relayPin2,1); digitalWrite(relayPin3,1);
nh.initNode(); nh.advertise(thrust); nh.subscribe(sub_left); nh.subscribe(sub_right); nh.subscribe(sub_center);
}
//
void loop(){
int rcValue1=pulseIn(rcChannelPin1,1); int throttlePWM1=map(rcValue1,1100,1900,1100,1900);
int rcValue2=pulseIn(rcChannelPin2,1); int throttlePWM2=map(rcValue2,1100,1900,-380, 380);
int rcValue3=pulseIn(receiverPin,1); int throttlePWM3=map(rcValue3,1100,1900,1100,1900);
int rcValue4=pulseIn(emergencyPin,1); 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); strip.show(); // R=255,G=0,B=0(빨간색)
digitalWrite(relayPin,0); digitalWrite(relayPin2,0); digitalWrite(relayPin3,0);
isAutonomousMode=false;
mode="EMERGENCY_STOP";
}
else{
if(throttlePWM3>1600){ // RC모드
for(int i=0;i<NUM_LEDS;i++)strip.setPixelColor(i,0,255,0); strip.show(); // R=0,G=255,B=0(초록색)
digitalWrite(relayPin,1); digitalWrite(relayPin2,1); digitalWrite(relayPin3,1);
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(throttlePWM3>1400){ // 자율운항모드
for(int i=0;i<NUM_LEDS;i++)strip.setPixelColor(i,255,255,0); strip.show(); delay(100); // R=255,G=255,B=0(노란색)
for(int i=0;i<NUM_LEDS;i++)strip.setPixelColor(i, 0, 0,0); strip.show(); delay(100); // 모든 LED 끄기
digitalWrite(relayPin,1); digitalWrite(relayPin2,1); digitalWrite(relayPin3,1);
isAutonomousMode=true;
char result_str[50];
snprintf(result_str,100,"MODE: AUTO_MODE");
str_msg.data=result_str;
thrust.publish(&str_msg);
}
else{ // 쓰러스터 제어 // if(throttlePWM3<=1400)
for(int i=0;i<NUM_LEDS;i++)strip.setPixelColor(i,0,0,0); strip.show(); delay(100); // 모든 LED 끄기
isAutonomousMode=false;
digitalWrite(relayPin,1); digitalWrite(relayPin2,0); digitalWrite(relayPin3,1);
}
}
nh.spinOnce();
delay(20);
}
//
if 조건식을 조금 변형했습니다.
(동작은 동일)
//
위 코드와 업로드된 회로도로 쓰러스터를 작동하려고 하는데
비상정지 모드만 작동이 되고
RC 및 자율운항모드는 작동이 되지 않습니다
if(throttlePWM4>1600){ // 비상정지모드
이 코드만 실행 된다면 throttlePWM4가 1600을 넘는지 확인하는 것이 우선되어야겠죠?시리얼추가해서 시리얼모니터에 출력해보세요//
LED또한 들어오지 않아서 답변 부탁드리겠습니다
LED가 켜지지 않는다는 것이 Neopixel을 의미하나요?Neopixel 예제를 돌려서 체크하시면 됩니다.
댓글 0
조회수 388등록된 댓글이 없습니다.