|
블루투스 통신 값이 시리얼 모니터에 출력이 안됩니다.
mctriangle | 2023-01-12
|
|
|---|---|
|
자이로 가속도 센서와 플랙스 센서를 이용서 로봇팔을 블루투스 통신으로 연결하려고 하는데 블루투스 통신은 되었다고 불은 들어오지만 서로 값을 읽고 출력하는 것이 안됩니다. 블루투스 모듈은 hc06 2개 입니다. ------------------------------------------------------------------------------------------------------------------------------ //----- MPU6050 -----//
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(5, 4); //블루투스의 Tx, Rx핀을 2번 3번핀으로 설정
const int thumb = A1;
const int pinkie = A2;
const int finger = A3;
MPU6050 mpu6050(Wire); // 서보모터 사용 선언(객체 생성)
int response_time = 1000;
void setup() {
Serial.begin(9600); // 9600 보레이트로 통신 시작
Wire.begin(); // I2C 통신 초기화(시작)
mpu6050.begin(); // mpu6050 초기화(시작)
mpu6050.calcGyroOffsets(true); // 오프셋 설정
pinMode(thumb, INPUT);
pinMode(pinkie, INPUT);
pinMode(finger, INPUT);
while (!Serial) {
; //시리얼통신이 연결되지 않았다면 코드 실행을 멈추고 무한 반복
}
mySerial.begin(9600);
}
void loop() {
int flexSensor = analogRead(thumb);//플랙스 센서 선언
int flexSensor1 = analogRead(pinkie);
int flexSensor2 = analogRead(finger);
if (mySerial.available()) { //블루투스에서 넘어온 데이터가 있다면
Serial.write(mySerial.read()); //시리얼모니터에 데이터를 출력
}
if (Serial.available()) { //시리얼모니터에 입력된 데이터가 있다면
mySerial.write(Serial.read()); //블루투스를 통해 입력된 데이터 전달
}
mpu6050.update(); // mpu6050모듈에서 값을 받아온 후 계산
if(flexSensor1>20){
Serial.print("G");//집게 접음
delay(response_time);
}else{
Serial.print("g");//집게 펼침
delay(response_time);
}
if(flexSensor>20){
Serial.print("T");//엄지 구부림
if(mpu6050.getAngleY()>30){
Serial.print("B");//베이스 완쪽 회전
delay(response_time);
}else if (mpu6050.getAngleY()<-30){
Serial.print("b");//베이스 오른쪽 회전
delay(response_time);}
if(mpu6050.getAngleX()>30){
Serial.print("S");//어깨 올림
delay(response_time);
}else if (mpu6050.getAngleX()<-30){
Serial.print("s");//어깨 내림
delay(response_time);}
}else{
Serial.print("t");//엄지
if(mpu6050.getAngleY()>30){
Serial.print("R");//roll left
delay(response_time);
}else if (mpu6050.getAngleY()<-30){
Serial.print("r");//roll right
delay(response_time);
}
}
if(flexSensor2>20){
Serial.print("F");//새끼 플랙스 센서 작동
if(mpu6050.getAngleX()>30){
Serial.println("E");//팔꿈치 앞
delay(response_time);
}else if (mpu6050.getAngleX()<-30){
Serial.println("e");//팔꿈치 뒤
delay(response_time);
}}else{
Serial.print("f");
if(mpu6050.getAngleX()>30){
Serial.println("P");//손목 핌
delay(response_time);
}else if (mpu6050.getAngleX()<-30){
Serial.println("p");//손목 접음
delay(response_time);
}}
}
----------------------------------------여기까지가 센서 코딩입니다.-------------------------------------------------------
#include <SoftwareSerial.h> // 블루투스 통신을 위한 SoftwareSerial 라이브러리를 불러온다.
#include "HCPCA9685.h"
#define I2CAdd 0x40
SoftwareSerial mySerial(2, 3); //블루투스의 Tx, Rx핀을 2번 3번핀으로 설정
HCPCA9685 HCPCA9685(I2CAdd);
//초기 모터 위치
const int servo_joint_0_parking_pos = 60;//base
const int servo_joint_1_parking_pos = 60;//shoulder
const int servo_joint_2_parking_pos = 70;//elbow
const int servo_joint_3_parking_pos = 47;//wrist roll
const int servo_joint_4_parking_pos = 63;//wrist pitch
const int servo_joint_5_parking_pos = 63;//gripper
//서보 모터 속도
int servo_joint_0_pos_increment = 20;
int servo_joint_1_pos_increment = 20;
int servo_joint_2_pos_increment = 20;
int servo_joint_3_pos_increment = 50;
int servo_joint_4_pos_increment = 60;
int servo_joint_5_pos_increment = 40;
//모터 위치 현재
int servo_joint_0_parking_pos_i = servo_joint_0_parking_pos;
int servo_joint_1_parking_pos_i = servo_joint_1_parking_pos;
int servo_joint_2_parking_pos_i = servo_joint_2_parking_pos;
int servo_joint_3_parking_pos_i = servo_joint_3_parking_pos;
int servo_joint_4_parking_pos_i = servo_joint_4_parking_pos;
int servo_joint_5_parking_pos_i = servo_joint_5_parking_pos;
//모터 최대-최소 값
int servo_joint_0_min_pos = 10;
int servo_joint_0_max_pos = 180;
int servo_joint_1_min_pos = 10;
int servo_joint_1_max_pos = 180;
int servo_joint_2_min_pos = 10;
int servo_joint_2_max_pos = 400;
int servo_joint_3_min_pos = 10;
int servo_joint_3_max_pos = 360;
int servo_joint_4_min_pos = 10;
int servo_joint_4_max_pos = 380;
int servo_joint_5_min_pos = 10;
int servo_joint_5_max_pos = 120;
//모터 포지션 초기값=0
int servo_0_pos = 0;
int servo_1_pos = 0;
int servo_joint_2_pos = 0;
int servo_joint_3_pos = 0;
int servo_joint_4_pos = 0;
int servo_joint_5_pos = 0;
char state = 0; // Changes value from ASCII to char
int response_time = 5;
int response_time_4 = 2;
int loop_check = 0;
int response_time_fast = 20;
int action_delay = 600;
// Define pin connections & motor's steps per revolution
const int dirPin = 4;
const int stepPin = 5;
const int stepsPerRevolution = 120;
int stepDelay = 4500;
const int stepsPerRevolutionSmall = 60;
int stepDelaySmall = 9500;
void setup(){
// 모터 드라이버 핀 선
pinMode(stepPin, OUTPUT);
pinMode(dirPin, OUTPUT);
//모터 라이브러리 불러오기
HCPCA9685.Init(SERVO_MODE);
//모터 드라이버 깨우기
HCPCA9685.Sleep(false);
Serial.begin(9600);
while (!Serial) {
; //시리얼통신이 연결되지 않았다면 코드 실행을 멈추고 무한 반복
}
Serial.println("Hello World!");
//블루투스와 아두이노의 통신속도를 9600으로 설정
mySerial.begin(9600);
delay(3000);
}
void loop(){
if (mySerial.available()) { //블루투스에서 넘어온 데이터가 있다면
Serial.write(mySerial.read()); //시리얼모니터에 데이터를 출력
}
if (Serial.available()) { //시리얼모니터에 입력된 데이터가 있다면
mySerial.write(Serial.read()); //블루투스를 통해 입력된 데이터 전달
}
state = mySerial.read(); // 수신 받은 데이터 저장
Serial.write(state); // 수신된 데이터 시리얼 모니터로 출력
//배이스 왼쪽 회전
if (state == 'B') {
baseRotateLeft();
delay(response_time);
}
//베이스 오른쪽 회전
if (state == 'b') {
baseRotateRight();
delay(response_time);
}
//어깨 내림
if (state == 's') {
shoulderServoForward();
delay(response_time);
}
//어깨 올림
if (state == 'S') {
shoulderServoBackward();
delay(response_time);
}
//팔꿈치 내림
if (state == 'e') {
elbowServoForward();
delay(response_time);
}
//팔꿈치 올림
if (state == 'E') {
elbowServoBackward();
delay(response_time);
}
//손목 올림
if (state == 'P') {
wristServoBackward();
delay(response_time);
}
//손목 내림
if (state == 'p') {
wristServoForward();
delay(response_time);
}
//손목 오른쪽 회전
if (state == 'r') {
wristServoCW();
delay(response_time);
}
//손목 왼쪽 회전
if (state == 'R') {
wristServoCCW();
delay(response_time);
}
//집게 펼침
if (state == 'g') {
gripperServoBackward();
delay(response_time);
}
//집게 접음
if (state == 'G') {
gripperServoForward();
delay(response_time);
}
}
void gripperServoForward() {//집게 접음
if (servo_joint_5_parking_pos_i > servo_joint_5_min_pos) {
HCPCA9685.Servo(5, servo_joint_5_parking_pos_i);
delay(response_time); //Delay the time takee to turn the servo by the given increment
Serial.println(servo_joint_5_parking_pos_i);
servo_joint_5_parking_pos_i = servo_joint_5_parking_pos_i - servo_joint_5_pos_increment;
}
}
void gripperServoBackward() {//집게 펼침
if (servo_joint_5_parking_pos_i < servo_joint_5_max_pos) {
HCPCA9685.Servo(5, servo_joint_5_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_5_parking_pos_i);
servo_joint_5_parking_pos_i = servo_joint_5_parking_pos_i + servo_joint_5_pos_increment;
}
}
void wristServoCW() {//손목 왼쪽 회전
if (servo_joint_3_parking_pos_i > servo_joint_3_min_pos) {
HCPCA9685.Servo(3, servo_joint_3_parking_pos_i);
delay(response_time_4);
Serial.println(servo_joint_3_parking_pos_i);
servo_joint_3_parking_pos_i = servo_joint_3_parking_pos_i - servo_joint_3_pos_increment;
}
}
void wristServoCCW() {//손목 오른쪽 회전
if (servo_joint_3_parking_pos_i < servo_joint_3_max_pos) {
HCPCA9685.Servo(3, servo_joint_3_parking_pos_i);
delay(response_time_4);
Serial.println(servo_joint_3_parking_pos_i);
servo_joint_3_parking_pos_i = servo_joint_3_parking_pos_i + servo_joint_3_pos_increment;
}
}
void wristServoForward() {//손목 내림
if (servo_joint_4_parking_pos_i < servo_joint_4_max_pos) {
HCPCA9685.Servo(4, servo_joint_4_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_4_parking_pos_i);
servo_joint_4_parking_pos_i = servo_joint_4_parking_pos_i + servo_joint_4_pos_increment;
}
}
void wristServoBackward() {//손목 올림
if (servo_joint_4_parking_pos_i > servo_joint_4_min_pos) {
HCPCA9685.Servo(4, servo_joint_4_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_4_parking_pos_i);
servo_joint_4_parking_pos_i = servo_joint_4_parking_pos_i - servo_joint_4_pos_increment;
}
}
void elbowServoForward() {//팔꿈치 내림
if (servo_joint_2_parking_pos_i < servo_joint_2_max_pos) {
HCPCA9685.Servo(2, servo_joint_2_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_2_parking_pos_i);
servo_joint_2_parking_pos_i = servo_joint_2_parking_pos_i + servo_joint_2_pos_increment;
}
}
void elbowServoBackward() {//팔꿈치 올림
if (servo_joint_2_parking_pos_i > servo_joint_2_min_pos) {
HCPCA9685.Servo(2, servo_joint_2_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_2_parking_pos_i);
servo_joint_2_parking_pos_i = servo_joint_2_parking_pos_i - servo_joint_2_pos_increment;
}
}
void shoulderServoForward() {//어깨 내림
if (servo_joint_1_parking_pos_i < servo_joint_1_max_pos) {
HCPCA9685.Servo(1, servo_joint_1_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_1_parking_pos_i);
servo_joint_1_parking_pos_i = servo_joint_1_parking_pos_i + servo_joint_1_pos_increment;
}
}
void shoulderServoBackward() {//어깨 올림
if (servo_joint_1_parking_pos_i > servo_joint_1_min_pos) {
HCPCA9685.Servo(1, servo_joint_1_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_1_parking_pos_i);
servo_joint_1_parking_pos_i = servo_joint_1_parking_pos_i - servo_joint_1_pos_increment;
}
}
void baseRotateLeft() {//베이스 왼쪽 회전
//clockwise
if (servo_joint_0_parking_pos_i > servo_joint_0_min_pos) {
HCPCA9685.Servo(0, servo_joint_0_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_0_parking_pos_i);
servo_joint_0_parking_pos_i = servo_joint_0_parking_pos_i - servo_joint_0_pos_increment;
}
}
void baseRotateRight() {//베이스 오른쪽 회전
//counterclockwise
if (servo_joint_0_parking_pos_i < servo_joint_0_min_pos) {
HCPCA9685.Servo(0, servo_joint_0_parking_pos_i);
delay(response_time);
Serial.println(servo_joint_0_parking_pos_i);
servo_joint_0_parking_pos_i = servo_joint_0_parking_pos_i - servo_joint_0_pos_increment;
}
}
-------------------------------------------------로봇팔 부분 코딩입니다.--------------------------------------------------
|
|
| 이전글 | 아두이노 버튼 눌렀을때 프로세싱에서 이미지가 바뀌게 하려면 어떻게 해야하나요? ... | 2022-12-08 |
| 다음글 | 고수님들 도와 주세요. 아두이노 나노 33 IOT 의 BLE 연결 문제 입니다.... | 2023-02-23 |
