[41호]ABT_System(Automatic Bicycle Transmission System)
2016 ICT 융합 프로젝트 공모전 입선
ABT_System(Automatic Bicycle Transmission System)
글 | 단국대학교 김승권, 서진욱, 이규호, 김영태, 최재호, 김재도
심사평
JK전자 최근 자전거를 이용하는 인구가 급격하게 늘어난 것은 사실이다. 대부분이 건강을 위한 운동이 목적일 것이다. 특히 자전거 운동은 런닝보다 무릎관절 등에 무리가 덜 가는 운동으로 알려져 있어 높은 연령층에서도 많이 이용을 하고 있는 것 같다. 관절 건강을 위한 최적의 속도와 기어비를 자동으로 조정해서 패달을 밟는 최적의 강도를 자동으로 설정을 해주는 기능이 있는 자전거라면 많은 수요가 있을 수 있을 것 같다.
뉴티씨 매우 실용적인 작품으로, 자전거가 외부에서 타는 것임을 감안하여 방수나 모터를 좀 더 튼튼한 것으로 사용하여 오랜기간 사용할 수 있도록 했으면 좋았을 것 같다. 충분히 실용화하여 사용할 수 있는 것으로 보이며, 재미있는 작품으로 보인다.
칩센 점점 수요가 늘고 있는 자전거를 이용한 모티브가 돋보입니다. 케이스를 직접 제작하는 열의와 작품의 완성도 또한 높다고 판단됩니다. 세부적으로 제품의 안전과 관련된 부분을 고려하여 보완한다면 상용화된 작품으로도 진행될 여지가 있습니다.
위드로봇 보고서의 내용 만으로는 잘 동작하였는지 판단하기 어렵습니다. 제작한 과정 뿐만 아니라 동작 상에서 발생하는 추가 문제점, 그 추가 문제점의 해결 방안에 대한 의견이 보고서에 첨부되면 더욱 좋겠습니다.
1. 작품 제목
ABT(Automatic Bicycle Transmission)_System
2. 작품 개요
자전거는 1800년대에 최초로 발명된 이래 인류 역사상 가장 멋진 발명품 중 하나이다. 2015년 기준으로 자전거 인구는 1200만명을 넘어섰으며 자동차와는 달리 연료없이 순수 인간의 동력으로만 구동 가능하다는 장점이 있어 친환경적이고 굉장히 경제적이다.
자전거는 본래 단순한 이동수단이었지만 철인 3종경기나 트라이애슬론과 같은 큰 경기가 열릴 정도로 인기가 높아졌다. 점점 더 가볍고 튼튼한 자전거가 개발되고 있으며 최근에는 버튼만으로도 변속이 가능한 자전거가 개발되었다. 변속은 자전거 주행을 할 때 안전과 연관된 부분이기 때문에 현재 달리고 있는 속도와 경사에 맞게, 빠르게 변속을 해야한다. 자전거에 익숙하지 않은 사람들은 변속하는 시점을 잘 모르기 때문에 무리하게 자전거를 탈 경우 무릎손상을 초래할 수도 있다. 현재 시중에 판매되는 자전거 중에서는 편리하고 기능이 많이 들어간 자전거 일수록 매우 비싸다. “비싼 자전거만 좋고 튼튼해야 할까?”라는 편견을 깨고 어느 자전거에나 장착이 가능한, 누구나 쉽게 변속을 할 수 있는, 편리한 자전거를 만들고 싶었다. 그래서 ABT_System을 제작하게 된 것이다.
3. 작품 설명
3.1. 개발환경
· 개발 언어 : C 언어, C++
· 개발 Tool : Arduino Sketch
· 사용 시스템 : 아두이노
현재 시중에 판매되고 있는 전자식 변속기는 크게 내장형 배터리 / 변속, 브레이크 레버 / 앞 변속기 / 뒷 변속기 / 통신케이블 / 배터리로 구성되어 있다. 물론 전자식 변속기를 장착하기 위해서는 장착할 수 있는 모델을 따로 구매해야하기 때문에 금전적인 문제가 상당히 크다. ABT_System을 장착하기 위해서는 자전거에 장착할 부분의 크기만 알면 되기 때문에 장착에 제한이 없다. 또한 구성품이 앞·뒤 변속기, LCD, 배터리, 컨트롤러, 스위치만 있으면 되기 때문에 구성품이 적으며, 장착이 간단하다.
ABT_System에는 LCD가 장착되어 있어 현재 변속기의 위치를 표시하며, 케이던스(1분당 페달을 밟는 횟수)와 속도를 동시에 확인할 수 있다. 바퀴와 페달에 장착된 마그네틱 스위치를 이용해 속도와 케이던스를 실시간으로 측정하며, 6축 가속도 센서를 이용하여 경사를 측정한다. 컨트롤러로 사용한 아두이노 나노를 통해 값을 계산한다. 이렇게 계산한 케이던스와 속도를 기준으로 적절한 기어비를 계산해 아두이노 메가로 기어를 변속하라는 명령을 전송한다. 전송받은 데이터를 기준으로 앞, 뒤 액추에이터 서보는 변속이 가능한 각도로 변경되며, 드레일러를 밀어 기어를 변속하게 된다.
3.2. 주행모드
ABT_System에는 총 3가지 모드가 존재하여 사용자가 원하는대로 변속이 가능하다.
3.2.1. One Finger Mode
One Finger Mode에서는 한 쪽 버튼만으로 변속이 가능하도록 소스코딩을 하였다. 이에 설정한 기어비는 앞, 뒤 변속기의 기어비를 계산하여 최적의 기어비로 변속이 가능하도록 설정하였다.
가장 표준 자전거는 앞 기어는 3개, 뒷 기어는 7개의 기어로 구성되어 있으며, 총 21단으로 변속이 가능하다. 자전거의 기어는 앞, 뒤 기어를 적절히 변속해야 현재 달리고 있는 환경에 따라 원활하게 주행이 가능하다. 예를 들어 앞 기어의 가장 작은 톱니를 1번, 가장 큰 톱니를 3번이라고 가정하고, 뒷 기어의 가장 작은 톱니를 1번, 가장 큰 톱니를 7번이라고 가정하자. 앞 기어가 1번일 때는 경사를 오르거나 저속 주행을 할 때 사용하는 데 뒷 기어는 5, 6, 7번 기어를 사용하는 것이 가장 적절하다. 즉 뒷 기어의 1, 2, 3, 4번은 사용할 확률이 적다는 뜻이다. One Finger Mode에서는 가장 적절한 기어비로 변속을 하는 것을 목적으로 하기 때문에 앞, 뒤 기어의 기어비를 계산하여 11가지의 경우의 수로 변속을 하게 된다.
3.2.2. Manual Mode
Manual Mode는 수동 변속 모드로 자동변속 기능을 필요로 하지 않고 사용자가 원하는 대로 즉각적으로 변속할 수 있는 설정이다. 좌측 버튼으로는 앞 기어의 변속을 담당하며, 우측 버튼으로는 뒷 기어의 변속을 담당한다. LCD에는 현재 변속 위치를 표시하기 때문에 사용자가 변속을 하고 싶을 때는 버튼을 눌러 실시간으로 변속이 가능하도록 설정하였다. 그렇다면 변속을 할 때 변속기의 위치는 고려하지 않을까? 일반 자전거의 변속기는 변속기 속선을 당기는 정도에 따라서 변속기의 위치를 세밀하게 조절이 가능하다. 변속기의 위치를 고려하지 않으면 체인이 대각선으로 연결되어 소음을 발생시키게 되는데 이 때 트리밍(Trimming)을 통해서 변속기의 위치를 조절해줘야 한다. 그래서 Manual Mode에서나, Automatic Mode, One Finger Mode에서도 트리밍이 가능하도록 코딩을 하여 변속을 할 때 소음이 발생하지 않도록 액추에이터가 이동한다. 트리밍을 하지 않으면 변속기의 플레이트가 닳아 손상되며, 체인에도 큰 영향을 미친다.
3.2.3. Automatic Mode
Automatic Mode는 ABT_System에 장착된 기능을 모두 활용하는 모드로 주행각도, 주행속도, 케이던스를 측정하여 현재 페달링 속도와 주행속도, 노면의 기울기를 모두 고려하여 가장 적절한 기어비로 변속을 할 수 있도록 설정하였다. 1차로 기울기, 2차로 주행속도, 3차로 케이던스를 고려한다. 변속을 할 때 가장 중요한 것이 노면의 기울기이다. 주행속도와 케이던스를 먼저 고려하게 되면 노면의 기울기에 따라서 변속이 먼저 이뤄지지 않기 때문에 안정성이 낮아진다.
만약 내리막을 주행하고 있으며 케이던스가 0인 상태에서는 어떻게 될까? 일정 수준의 케이던스를 넘지 않으면 변속을 하지 않으며, 변속을 하던 도중에 페달링을 멈추면 케이던스가 0이 되기 때문에 변속을 하지 않도록 설정하였다. 만약 케이던스가 0이더라도 주행 중이거나 정지하는 경우에는 이전에 유지한 변속 값을 유지하기 때문에 갑자기 변속이 되지 않는다.
3.3. LCD 모듈
3.3.1. 부품리스트
3.3.2. 부품 설명
캐릭터 LCD : 캐릭터 LCD는 그래픽 LCD와는 달리 문자 하나당 40개의 픽셀로 이뤄져 있으며, 아두이노나 Atmega128과 같은 MCU에서는 헤더파일에 기본적인 알파벳 코드가 작성되어 있어 함수를 호출하여 문자를 입력하면 바로 LCD에 글자를 띄울 수 있다.
MC7805CT : MC7805CT는 회로에서 요구하는 전압이 있어 일정 수준 이하로 전압을 내릴 때 사용하는 리니어 레귤레이터이다. 정전압 레귤레이터에는 스위칭 방식인 SMPS, 가장 많이 사용되는 리니어 레귤레이터가 있다. 리니어 레귤레이터는 전위차를 열에너지로 소모하기 때문에 에너지 효율은 좋지 않지만 회로구성이 간단하고 요구전압과 입력전압의 차이가 많이 나지않을 때 사용된다. 모델명의 뒷자리 숫자, 즉 78XX는 변환하고자하는 전압을 의미하며 우리가 사용하는 7805는 5V로 전압을 내려주는 리니어 레귤레이터이다.
아두이노 나노 : 아두이노 나노는 아두이노 프로 미니 다음으로 작은 모델이다. 아두이노 우노나 메가와 같은 모델은 크기가 크기 때문에 한정된 크기에서 MCU를 요구할 때는 적합하지 않다는 단점이 있다. 때문에 아두이노 나노나 프로 미니, 마이크로와 같은 모델을 사용하며, Atmega328을 사용하여 아두이노 우노나 마이크로, 프로 미니와 다른 점이 거의 없다.
마그네틱 센서 : 마그네틱 센서는 비활성 가스를 충전한 유리관 속에 봉입된 스위치를 의미한다. 리드스위치는 다른 스위치와 달리 접점의 동작, 복귀가 빠르고 신뢰도가 높기 때문에 TACT 스위치보다 사용하기 편하다. 자전거에 장착하여 고속으로 회전하는 케이던스와 바퀴의 속도를 측정하기 위해서는 거리측정센서나 근접센서를 사용하기 어렵다. 때문에 자력에 의해 스위치가 On/Off되는 마그네틱 센서를 사용하는 것이 인식률이나 장착에 더 효율적이다.
3.4. 앞·뒤 변속기 / 메인 컨트롤러
3.4.1. 부품리스트
3.4.2. 부품 설명
아두이노 메가 : 아두이노 메가는 Atmega2560을 사용하고 GPIO 포트가 다양하여 확장성이 좋다는 장점이 있다. 아두이노 나노 등을 사용해 소형화할 수 있었는데 아두이노 메가를 사용한 이유는 다음과 같다.
아두이노 메가 이외의 아두이노 보드는 디지털/아날로그 포트, Hardware Serial Port가 적기 때문에 많은 GPIO 포트를 요구할 때는 적합하지 않다. ABT_System은 Hardware Serial Port가 총 3개 필요하다. AX-12A를 구동할 때 Serial 통신으로 Packet을 보내주는데 제작과정에서 SoftwareSerial 클래스를 사용하는 것이 되지 않아 Hardware Serial Port를 사용해야 했다. 그래서 불가피하게 Atmega2560을 선택했으며, 포트 부족 문제를 해결하였다.
AX-12A : AX-12A 액추에이터 서보는 내부에 MCU가 장착되어 수신하는 Packet에 의해 동작한다. Robotics에서 제공하는 데이터시트를 보면 Instruction마다 기능을 다르게 설정할 수 있으며, ABT_System에는 360도 회전하는 주행모드보다는 각도 제어모드가 적합하기 때문에 AX-12A를 선택하였다. 일반 디지털 서보보다 비싼 가격대가 형성되어 있지만 토크, 허용전류, 빠른 응답속도, 정확한 명령 수행으로 인해 위 액추에이터를 선택하였다. 또한 일반 서보는 180도까지 제어가 가능하지만 액추에이터 서보는 330도까지 제어가 가능하기 때문에 더 넓은 제어각도를 처리할 수 있다.
MMA8452 : MMA8452는 3축 가속도센서로 X, Y, Z축에서 기울어진 정도를 측정하여 각도로 표시를 해주기 때문에 기울기를 측정하기에 적합한 모듈이다. 보통 가속도, 자이로센서는 SPI, I2C 통신을 둘 다 지원하기 때문에 사용자가 원하는 대로 제어가 가능하다는 장점이 있다. 자이로 센서와 가속도 센서의 차이점은 측정하는 값에 차이가 있다. 가속도 센서는 X, Y, Z축의 중력가속도 성분을 분해하여 각각 각도로 나타내는 것이 가능하다. 자이로 센서는 각 축의 각속도를 측정하는 것이기 때문에 각도를 구하려면 적분을 해야한다. 때문에 ABT_System을 구현할 때는 가속도 센서를 사용하는 것이 적합하다고 판단하였다. ABT_System에서는 주행 각도를 측정하여 그에 알맞은 기어비로 기어를 변속하게 된다.
4. 단계별 제작과정
4.1. ABT_System 장착 위치별 지름, 세부크기 측정
4.2. 부품별 3D Modeling
4.3. 3D Printing
4.4. 2차, 3차 모델링 수정
4.5. 최종 출력 및 부품배치
4.6. 부품 조립 및 자전거 장착
4.7. 테스트
4.8. 회로도
4.9. 시험주행
5. 소스코드
5.1. FRONT CODE
FRONT CODE
#include <LiquidCrystal.h>
#include <Wire.h>
#include <SFE_MMA8452Q.h>
#include <String.h>
#define r_up 10
#define r_down 11
#define f_up 12
#define f_down 13
// Button Setting pin
#define f_7 1
#define f_6 2
#define s_7 3
#define s_6 4
#define f_5 5
#define t_7 6
#define t_6 7
#define s_5 8
#define f_4 9
#define t_5 10
#define s_4 11
int mode_state=1;
int m_after1, m_after2 = 0;
int mode = 0;
int mode_temp=0;
int in[4] = {};
float in_spd = 0;
unsigned long time,time2, oldtime,oldtime2, delta,delta2;
float spd1,spd2 = 0;
float temp1, temp2 =0;
int swpin=18,swpin2=19, swval=0,swval2=0, oldswval=0,oldswval2=0;
LiquidCrystal lcd(7,6,5,4,3,2);
int after[4], before[4] = {0};
int f_state =1;
int r_state = 1;
int spd, rpm =0;
int s_temp, r_temp = 0;
char f_i2c;
char r_i2c;
char out[5]={};
MMA8452Q accel;
void setup()
{
accel.init();
lcd.begin(16,2);
pinMode(swpin,INPUT);
pinMode(swpin2,INPUT);
pinMode(r_up, INPUT);
pinMode(r_down,INPUT);
pinMode(f_up, INPUT);
pinMode(f_down, INPUT);
Serial.begin(9600);
}
void loop()
{
speed_sens(spd1, spd2);
spd = (int)spd1;
rpm = (int)spd2;
switch(mode)
{
case 0:
{
lcd.setCursor(0,0);
lcd.print(“Select Mode”);
button();
mode = mode_change(in[0],in[2], in[3]);
break;
}
case 1:
{
button();
Mode_1();
lcd_cursor_mode1();
break;
}
case 2:
{
button();
Mode_2();
lcd_cursor_mode2();
break;
}
case 3:
{
button();
Mode_3();
lcd_cursor_mode3();
break;
}
default:
{}
}
f_i2c = trans(f_state);
r_i2c = trans(r_state);
out[0] = f_i2c;
out[1] = ‘+’;
out[2] = r_i2c;
out[3] = ‘\n’;
out[4] = ”;
Serial.print(out);
delay(150);
if(mode_temp != mode)
{
lcd.clear();
}
mode_temp = mode;
s_temp = spd;
r_temp = rpm;
}
void lcd_spd()
{
lcd.print(“SPD:”);
if(spd<10)
{
lcd.setCursor(5,1);
lcd.print(” “);
lcd.setCursor(4,1);
lcd.print(spd);
}
else if(spd>=10)
{
lcd.setCursor(6,1);
lcd.print(” “);
lcd.setCursor(4,1);
lcd.print(spd);
}
lcd.setCursor(7,1);
lcd.print(” RPM:”);
if(rpm>=10)
{
lcd.setCursor(14,1);
lcd.print(” “);
lcd.setCursor(12,1);
lcd.print(rpm);
}
else if(rpm<10)
{
lcd.setCursor(13,1);
lcd.print(” “);
lcd.setCursor(12,1);
lcd.print(rpm);
}
else if(rpm>99)
{
lcd.setCursor(12,1);
lcd.print(rpm);
}
}
void lcd_cursor_mode2()
{
//lcd.clear();
lcd.setCursor(0,0);
lcd.print(“MLM”);
lcd.print(” Fr:”);
lcd.print(f_state);
lcd.print(“, Rr:”);
lcd.print(r_state);
lcd.setCursor(0,1);
lcd_spd();
}
void lcd_cursor_mode1()
{
lcd.setCursor(0,0);
lcd.print(“ATM”);
lcd.print(” Fr:”);
lcd.print(f_state);
lcd.print(“, Rr:”);
lcd.print(r_state);
lcd.setCursor(0,1);
lcd.print(“SPD:”);
lcd.print(spd);
lcd.print(” RPM:”);
lcd.print(rpm);
lcd_spd();
}
void lcd_cursor_mode3()
{
lcd.setCursor(0,0);
lcd.print(“OFM”);
lcd.print(” Fr:”);
lcd.print(f_state);
lcd.print(“, Rr:”);
lcd.print(r_state);
lcd.setCursor(0,1);
lcd.print(“SPD:”);
lcd.print(spd);
lcd.print(” RPM:”);
lcd.print(rpm);
lcd_spd();
}
void button()
{
// int in[4] = {0};
int i = 0;
for(i=0;i<4;i++)
{
in[i] = digitalRead(10+i);
}
if((in[0] == 1) && (in[2] == 1))
{
mode = 0;
}
}
char trans(int state)
{
switch(state)
{
case 1:
{
return ’1′;
}
case 2:
{
return ’2′;
}
case 3:
{
return ’3′;
}
case 4:
{
return ’4′;
}
case 5:
{
return ’5′;
}
case 6:
{
return ’6′;
}
case 7:
{
return ’7′;
}
default:
{
return ’0′;
}
}
}
int mode_change(int in1, int in2, int in3)
{
if(in1 == 1)
{
return 1;
}
if(in2 == 1)
{
return 2;
}
if(in3 == 1)
{
return 3;
}
}
int Mode_1()
{
if(accel.x < 15 && accel.x>=0)
{
if(spd != 0 && rpm < 30)
{
spd = s_temp;
rpm = r_temp;
}
if(spd >=0 && spd<=15)
{
if(rpm<=30)
{
mode_state = 1;
}
else if(rpm >30 && rpm<=60)
{
mode_state = 2;
}
else if(rpm > 60 && rpm<= 90)
{
mode_state = 3;
}
}
else if(spd >15 && spd<=25)
{
if(rpm<=30)
{
mode_state = 4;
}
else if(rpm >30 && rpm<=60)
{
mode_state = 5;
}
else if(rpm > 60 && rpm<= 90)
{
mode_state = 6;
}
else if(rpm >90 && rpm <= 120)
{
mode_state = 7;
}
}
else if(spd > 25)
{
if(rpm<=30)
{
mode_state = 8;
}
else if(rpm >30 && rpm<=60)
{
mode_state = 9;
}
else if(rpm > 60 && rpm<= 90)
{
mode_state = 10;
}
else if(rpm >90 && rpm <= 120)
{
mode_state = 11;
}
}
}
else if(accel.x >15)
{
if(spd >=0 && spd<=15)
{
if(rpm<=30)
{
mode_state = 1;
}
else if(rpm >30 && rpm<=60)
{
mode_state = 2;
}
else if(rpm > 60 && rpm<= 90)
{
mode_state = 3;
}
}
}
else if(accel.x < 0 && accel.x >-15)
{
if(spd >15 && spd<=25)
{
if(rpm<=30)
{
mode_state = 4;
}
else if(rpm >30 && rpm<=60)
{
mode_state = 5;
}
else if(rpm > 60 && rpm<= 90)
{
mode_state = 6;
}
else if(rpm >90 && rpm <= 120)
{
mode_state = 7;
}
}
}
switch(mode_state)
{
case 1:
{
f_state = 1;
r_state = 7;
return 0;
}
case 2:
{
f_state = 1;
r_state = 6;
return 0;
}
case 3:
{
f_state = 1;
r_state = 5;
return 0;
}
case 4:
{
f_state = 2;
r_state = 6;
return 0;
}
case 5:
{
f_state = 2;
r_state = 5;
return 0;
}
case 6:
{
f_state = 2;
r_state = 4;
return 0;
}
case 7:
{
f_state = 2;
r_state = 3;
return 0;
}
case 8:
{
f_state = 3;
r_state = 5;
return 0;
}
case 9:
{
f_state = 3;
r_state = 4;
return 0;
}
case 10:
{
f_state = 3;
r_state = 3;
return 0;
}
case 11:
{
f_state = 3;
r_state = 2;
return 0;
}
default:
{
}
}
}
void Mode_2()
{
int j=0;
if(after[0] != 1)
{
if(in[2] == 1 && r_state < 7)
{
r_state += 1;
}
}
if(after[1] != 1)
{
if(in[3] == 1 && r_state >1 )
{
r_state -= 1;
}
}
if(after[2] != 1)
{
if(in[0] == 1 && f_state < 3)
{
f_state += 1;
}
}
if(after[3] != 1)
{
if(in[1] == 1 && f_state > 1)
{
f_state -= 1;
}
}
for(j=0;j<4;j++)
{
before[j] = in[j];
after[j] = before[j];
}
}
int Mode_3()
{
int Read1 = digitalRead(r_up);
int Read2 = digitalRead(r_down);
if(m_after1 != 1)
{
if((mode_state < 21) && (in[2] == 1))
{
mode_state += 1;
}
}
if(m_after2 != 1)
{
if((mode_state > 1) && (in[3] == 1))
{
mode_state -= 1;
}
}
m_after1 = in[2];
m_after2 = in[3];
switch(mode_state)
{
case 1:
{
f_state = 1;
r_state = 7;
return 0;
}
case 2:
{
f_state = 1;
r_state = 6;
return 0;
}
case 3:
{
f_state = 1;
r_state = 5;
return 0;
}
case 4:
{
f_state = 2;
r_state = 6;
return 0;
}
case 5:
{
f_state = 2;
r_state = 5;
return 0;
}
case 6:
{
f_state = 2;
r_state = 4;
return 0;
}
case 7:
{
f_state = 2;
r_state = 4;
return 0;
}
case 8:
{
f_state = 3;
r_state = 5;
return 0;
}
case 9:
{
f_state = 3;
r_state = 4;
return 0;
}
case 10:
{
f_state = 3;
r_state = 3;
return 0;
}
case 11:
{
f_state = 3;
r_state = 2;
return 0;
}
default:
{
}
}
}
void speed_sens(float in1, float in2)
{
accel.read();
swval = digitalRead(swpin);
swval2 = digitalRead(swpin2);
if(((oldswval2 == LOW) && (swval2 == HIGH)))
{
time = millis();
delta = millis() – oldtime;
//26″ * 25.4 * 3.141592 = 2.074m, 0.002174 km;
// * 3600 = 7446.4 km/s
in1 = 7600 / (float)delta;
oldtime = time;
// Serial.print((int)spd + “+” + (int)spd2);
}
if((oldswval == LOW) && (swval == HIGH))
{
time2 = millis();
delta2 = millis() – oldtime2;
in2 = ((1.0 / (float)delta2) * 60000);
oldtime2 = time2;
}
// if((spd != 0 || spd2 != 0) && ((temp1 != spd )|| (temp2 != spd2)))
//{
// sprintf(buf, “%3ld+%3ld”, (long)spd, (long)spd2);
// Serial.println(buf);
// Serial.print(buf);
//}
oldswval = swval;
oldswval2 = swval2;
temp1 = in1;
temp2 = in2;
}
5.2. REAR CODE
REAR CODE
#include <String.h>
char Recv[5]={};
byte rearD = 0×02;
byte frontD = 0×01;
int frontAngle, rearAngle = 0;
char front_n,rear_n = ’1′;
char temp_f, temp_r = ’1′;
unsigned long time,time2, oldtime,oldtime2, delta,delta2;
float spd,spd2 = 0;
float temp1, temp2 =0;
int swpin=8,swpin2=9, swval=0,swval2=0, oldswval=0,oldswval2=0;
char buf[7] = {0};
void setup() {
pinMode(2, OUTPUT);
pinMode(swpin,INPUT);
pinMode(swpin2,INPUT);
digitalWrite(2,LOW);
Serial.begin(9600);
Serial1.begin(1000000);
Serial2.begin(1000000);
}
//400 1단 , 540 7단
void loop() {
int i=0;
while(Serial.available())
{
char data = SW.read();
Recv[i] = data;
i++;
}
change_value_f();
change_value_r();
Packet1(0×01,frontAngle);
Packet2(0×01,rearAngle);
swval = digitalRead(swpin);
swval2 = digitalRead(swpin2);
if(((oldswval2 == LOW) && (swval2 == HIGH)))
{
time = millis();
delta = millis() – oldtime;
//26″ * 25.4 * 3.141592 = 2.074m, 0.002174 km;
// * 3600 = 7446.4 km/s
spd = 7600 / (float)delta;
oldtime = time;
}
if((oldswval == LOW) && (swval == HIGH))
{
time2 = millis();
delta2 = millis() – oldtime2;
spd2 = ((1.0 / (float)delta2) * 60000);
oldtime2 = time2;
}
if((spd != 0 || spd2 != 0) && ((temp1 != spd )|| (temp2 != spd2)))
{
sprintf(buf, “%3ld+%3ld”, (long)spd, (long)spd2);
Serial.print(buf);
}
oldswval = swval;
oldswval2 = swval2;
temp1 = spd;
temp2 = spd2;
delay(50);
}
void Packet1(unsigned char ID, int Angle)
{
unsigned char Check_Sum, Header = 0xFF;
Serial1.write(Header);
Serial1.write(Header);
Serial1.write(ID); // Motor ID
Serial1.write(0×05); // Data legnth
Serial1.write(0×03); // Instruction
Serial1.write(0x1E); // Goal Position
Serial1.write(Angle); // Set Goal Position
Serial1.write(Angle>>8); //Clear Goal position 8bit
Check_Sum = ~(ID + 0×05 + 0×03 + 0x1E + Angle + (Angle>>8)); // Send Data Error Check
Serial1.write(Check_Sum);
}
void change_value_r()
{
if(rear_n != Recv[2])
{
rearAngle = rear_transferid(Recv[2]);
}
else
{
rearAngle = rear_transferid(rear_n);
}
//temp_r = rear_n;
rear_n = Recv[2];
// Recv[2] = temp_r;
}
void change_value_f()
{
if(front_n != Recv[0])
{
frontAngle = front_transferid(Recv[0]);
}
else
{
frontAngle = front_transferid(front_n);
}
temp_f = front_n;
front_n = Recv[0];
// Recv[0] = temp_f;
}
void Packet2(unsigned char ID, int Angle)
{
unsigned char Check_Sum, Header = 0xFF;
Serial2.write(Header);
Serial2.write(Header);
Serial2.write(ID); // Motor ID
Serial2.write(0×05); // Data legnth
Serial2.write(0×03); // Instruction
Serial2.write(0x1E); // Goal Position
Serial2.write(Angle); // Set Goal Position
Serial2.write(Angle>>8); //Clear Goal position 8bit
Check_Sum = ~(ID + 0×05 + 0×03 + 0x1E + Angle + (Angle>>8)); // Send Data Error Check
Serial2.write(Check_Sum);
}
void Default(unsigned char ID1)
{
Packet1(ID1, 512);
delay(250);
}
int rear_transferid(char a)
{
switch(a){
case ’1′: {return 440;}
case ’2′: {return 500;}
case ’3′: {return 520;}
case ’4′: {return 540;}
case ’5′: {return 580;}
case ’6′: {return 610;}
case ’7′: {return 700;}
default : {return 480;}
}
}
int front_transferid(char a)
{
switch(a){
case ’1′: {return 250;}
case ’2′: {return 530;}
case ’3′: {return 690;}
default : {}
}
}
void First_front()
{
Packet1(0×01,512);
}
void Second_front()
{
Packet1(0×01,1023);
delay(500);
Packet1(0×01,750);
}
void Third_front()
{
Packet1(0×01,1023);
}