Login▼/회원가입BASIC4MCU | 질문게시판 | 반복코드 수정하고싶습니다.
페이지 정보
작성자 민수 작성일2019-08-26 13:53 조회7,636회 댓글0건본문
#include <DynamixelWorkbench.h>
#include <math.h>
#define DXL_BUS_SERIAL3 "3" //Dynamixel on Serial3(USART3) <-OpenCM 485EXP
#define DXL_BUS_SERIAL4 "/dev/ttyUSB0" //Dynamixel on Serial3(USART3) <-OpenCR
//////////////////////////////////////////////////////////////////////////////////////////////////
#define BAUDRATE 57600
#define DXL_ID1 1
#define DXL_ID2 2
#define DXL_ID3 3
#define DXL_ID4 4
#define DXL_ID5 5
#define DXL_ID6 6
//////////////////////////////////////////////////////////////////////////////////////////////////
int32_t get_data1 = 0; int32_t get_data2 = 0; int32_t get_data3 = 0;
int32_t get_data4 = 0; int32_t get_data5 = 0; int32_t get_data6 = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////
uint8_t dxl_id[6] = {DXL_ID1, DXL_ID2, DXL_ID3, DXL_ID4, DXL_ID5, DXL_ID6};
const uint8_t handler_index = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////
DynamixelWorkbench dxl_wb;
//////////////////////////////////////////////////////////////////////////////////////////////////
int load1 = A0; int load2 = A1; int load3 = A2; int load4 = A3; int load5 = A4; int load6 = A5;
//////////////////////////////////////////////////////////////////////////////////////////////////
int readload1 = 0; int readload2 = 0; int readload3 = 0; int readload4 = 0; int readload5 = 0; int readload6 = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////
int total_1 = 0; int total_2 = 0; int total_3 = 0;
int total_4 = 0; int total_5 = 0; int total_6 = 0; //측정한 값들의 전체합
//////////////////////////////////////////////////////////////////////////////////////////////////
int average1 = 0; int average2 = 0; int average3 = 0;
int average4 = 0; int average5 = 0; int average6 = 0; //측정한 값들의 평균값
//////////////////////////////////////////////////////////////////////////////////////////////////
float power1 = 0; float power2 = 0; float power3 = 0;
float power4 = 0; float power5 = 0; float power6 = 0; // avr - 600 , 550 - avr
float pgain1 = 0; float pgain2 = 0; float pgain3 = 0;
float pgain4 = 0; float pgain5 = 0; float pgain6 = 0;
int minus = 570; int plus = 580;
void setup()
{
Serial.begin(57600);
dxl_wb.begin(DXL_BUS_SERIAL4, BAUDRATE);
for (int cnt = 0; cnt < 6; cnt++)
{
dxl_wb.ping(dxl_id[cnt]);
dxl_wb.currentBasedPositionMode(dxl_id[cnt], 150);
//dxl_wb.goalPosition(dxl_id[cnt], (int32_t)0);
delay(200);
}
}
void loop()
{
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
dxl_wb.itemRead(DXL_ID1, "Present_Position", &get_data1); dxl_wb.itemRead(DXL_ID2, "Present_Position", &get_data2);
dxl_wb.itemRead(DXL_ID3, "Present_Position", &get_data3); dxl_wb.itemRead(DXL_ID4, "Present_Position", &get_data4);
dxl_wb.itemRead(DXL_ID5, "Present_Position", &get_data5); dxl_wb.itemRead(DXL_ID6, "Present_Position", &get_data6);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Serial.print(0); Serial.print(" "); Serial.print(1023); Serial.print(" ");
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
total_1 = 0; total_2 = 0; total_3 = 0; total_4 = 0; total_5 = 0; total_6 = 0;
for (int avr = 0; avr < 20; avr++)
{
readload1 = analogRead(load1); readload2 = analogRead(load2); readload3 = analogRead(load3);
readload4 = analogRead(load4); readload5 = analogRead(load5); readload6 = analogRead(load6);
total_1 = total_1 + readload1; total_2 = total_2 + readload2; total_3 = total_3 + readload3;
total_4 = total_4 + readload4; total_5 = total_5 + readload5; total_6 = total_6 + readload6;
}
average1 = total_1/20; average2 = total_2/20; average3 = total_3/20;
average4 = total_4/20; average5 = total_5/20; average6 = total_6/20;
for (int clr = 0; clr < 200; clr++)
{
float pgain1 = 0; float pgain2 = 0; float pgain3 = 0;
float pgain4 = 0; float pgain5 = 0; float pgain6 = 0;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Serial.print("1번 로드셀 : "); Serial.print(average1); Serial.print(" ");
Serial.print("2번 로드셀 : "); Serial.print(average2); Serial.print(" ");
Serial.print("3번 로드셀 : "); Serial.print(average3); Serial.print(" ");
Serial.print("4번 로드셀 : "); Serial.print(average4); Serial.print(" ");
Serial.print("5번 로드셀 : "); Serial.print(average5); Serial.print(" ");
Serial.print("6번 로드셀 : "); Serial.print(average6); Serial.print(" ");
Serial.println();
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
if (average1 < minus){ //R
power1 = minus - average1;
pgain1 = pow(0.04*power1,2.1)-70;
pgain1 = constrain(pgain1,10,200);
get_data1 = get_data1+pgain1;
dxl_wb.goalPosition(DXL_ID1, get_data1);
Serial.print("1 MX64 present position : "); Serial.print(get_data1); Serial.println(")");
get_data1 = get_data1;
dxl_wb.goalPosition(DXL_ID1, get_data1);
}
else if (average1 > minus && average1 < plus){
get_data1 = get_data1;
dxl_wb.goalPosition(DXL_ID1, get_data1);
Serial.print("1 MX64 present position : "); Serial.print(get_data1); Serial.println(")");
}
else {
power1 = average1 - plus;
pgain1 = pow(0.04*power1,2.1)-200;
pgain1 = constrain(pgain1,10,200);
get_data1 = get_data1-pgain1;
dxl_wb.goalPosition(DXL_ID1, get_data1);
Serial.print("1 MX64 present position : "); Serial.print(get_data1); Serial.println(")");
get_data1 = get_data1;
dxl_wb.goalPosition(DXL_ID1, get_data1);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (average2 < minus){ //L
power2 = minus - average2;
pgain2 = pow(0.04*power2,2.1)-70;
pgain2 = constrain(pgain2,10,200);
get_data2 = get_data2-pgain2;
dxl_wb.goalPosition(DXL_ID2, get_data2);
Serial.print("2 MX64 present position : "); Serial.print(get_data2); Serial.println(")");
get_data2 = get_data2;
dxl_wb.goalPosition(DXL_ID2, get_data2);
}
else if (average2 > minus && average2 < plus){
get_data2 = get_data2;
dxl_wb.goalPosition(DXL_ID2, get_data2);
Serial.print("2 MX64 present position : "); Serial.print(get_data2); Serial.println(")");
}
else {
power2 = average2 - plus;
pgain2 = pow(0.04*power2,2.1)-200;
pgain2 = constrain(pgain2,10,200);
get_data2 = get_data2+pgain2;
dxl_wb.goalPosition(DXL_ID2, get_data2);
Serial.print("2 MX64 present position : "); Serial.print(get_data2); Serial.println(")");
get_data2 = get_data2;
dxl_wb.goalPosition(DXL_ID2, get_data2);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (average3 < minus){ //R
power3 = minus - average3;
pgain3 = pow(0.04*power3,2.1)-70;
pgain3 = constrain(pgain3,10,200);
get_data3 = get_data3+pgain3;
dxl_wb.goalPosition(DXL_ID3, get_data3);
Serial.print("3 MX64 present position : "); Serial.print(get_data3); Serial.println(")");
get_data3 = get_data3;
dxl_wb.goalPosition(DXL_ID3, get_data3);
}
else if (average3 > minus && average3 < plus){
get_data3 = get_data3;
dxl_wb.goalPosition(DXL_ID3, get_data3);
Serial.print("3 MX64 present position : "); Serial.print(get_data3); Serial.println(")");
}
else {
power3 = average3 - plus;
pgain3 = pow(0.04*power3,2.1)-200;
pgain3 = constrain(pgain3,10,200);
get_data3 = get_data3-pgain3;
dxl_wb.goalPosition(DXL_ID3, get_data3);
Serial.print("3 MX64 present position : "); Serial.print(get_data3); Serial.println(")");
get_data3 = get_data3;
dxl_wb.goalPosition(DXL_ID3, get_data3);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (average4 < minus){ //L
power4 = minus - average4;
pgain4 = pow(0.04*power4,2.1)-70;
pgain4 = constrain(pgain4,10,200);
get_data4 = get_data4-pgain4;
dxl_wb.goalPosition(DXL_ID4, get_data4);
Serial.print("4 MX64 present position : "); Serial.print(get_data4); Serial.println(")");
get_data4 = get_data4;
dxl_wb.goalPosition(DXL_ID4, get_data4);
}
else if (average4 > minus && average4 < plus){
get_data4 = get_data4;
dxl_wb.goalPosition(DXL_ID4, get_data4);
Serial.print("4 MX64 present position : "); Serial.print(get_data4); Serial.println(")");
}
else {
power4 = average4 - plus;
pgain4 = pow(0.04*power4,2.1)-200;
pgain4 = constrain(pgain4,10,200);
get_data4 = get_data4+pgain4;
dxl_wb.goalPosition(DXL_ID4, get_data4);
Serial.print("4 MX64 present position : "); Serial.print(get_data4); Serial.println(")");
get_data4 = get_data4;
dxl_wb.goalPosition(DXL_ID4, get_data4);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (average5 < minus){ //R
power5 = minus - average5;
pgain5 = pow(0.04*power5,2.1)-70;
pgain5 = constrain(pgain5,10,200);
get_data5 = get_data5+pgain5;
dxl_wb.goalPosition(DXL_ID5, get_data5);
Serial.print("5 MX64 present position : "); Serial.print(get_data5); Serial.println(")");
get_data5 = get_data5;
dxl_wb.goalPosition(DXL_ID5, get_data5);
}
else if (average5 > minus && average5 < plus){
get_data5 = get_data5;
dxl_wb.goalPosition(DXL_ID5, get_data5);
Serial.print("5 MX64 present position : "); Serial.print(get_data5); Serial.println(")");
}
else {
power5 = average5 - plus;
pgain5 = pow(0.04*power5,2.1)-200;
pgain5 = constrain(pgain5,10,200);
get_data5 = get_data5-pgain5;
dxl_wb.goalPosition(DXL_ID5, get_data5);
Serial.print("5 MX64 present position : "); Serial.print(get_data5); Serial.println(")");
get_data5 = get_data5;
dxl_wb.goalPosition(DXL_ID5, get_data5);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (average6 < minus){ //L
power6 = minus - average6;
pgain6 = pow(0.04*power6,2.1)-70;
pgain6 = constrain(pgain6,10,200);
get_data6 = get_data6-pgain6;
dxl_wb.goalPosition(DXL_ID6, get_data6);
Serial.print("6 MX64 present position : "); Serial.print(get_data6); Serial.println(")");
get_data6 = get_data6;
dxl_wb.goalPosition(DXL_ID6, get_data6);
}
else if (average6 > minus && average6 < plus){
get_data6 = get_data6;
dxl_wb.goalPosition(DXL_ID6, get_data6);
Serial.print("6 MX64 present position : "); Serial.print(get_data6); Serial.println(")");
}
else {
power6 = average6 - plus;
pgain6 = pow(0.04*power6,2.1)-200;
pgain6 = constrain(pgain6,10,200);
get_data6 = get_data6+pgain6;
dxl_wb.goalPosition(DXL_ID6, get_data6);
Serial.print("6 MX64 present position : "); Serial.print(get_data6); Serial.println(")");
get_data6 = get_data6;
dxl_wb.goalPosition(DXL_ID6, get_data6);
}
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
}
@@@@@ 시작이 1번모터 구동코드입니다. 마지막@@@는 6번모터 구동코드이구요.
각 구동코드 1번부터 6번까지 거의 다 같습니다.
단지 L이나 R이냐에따라 +와 -만 바뀌고 변수의 숫자만 바뀐형태인데요.
이걸 간단하게 변수로써 압축시킬 수 있을까요????
L - 2,4,6번 모터 // R - 1,3,5번 모터코드이니 총 2개로 압축시킬 수 있다면 좋을 것 같습니다.
관련 예제나 간단한 문구로라도 설명해주시면 감사하겠습니다.
- BASIC4MCU 작성글 SNS에 공유하기
댓글 0
조회수 7,636등록된 댓글이 없습니다.