13. 원격제어 로봇 - SBUS 이용

▼ 회로도

▼ 소스 코드

#include <U8glib.h>
#include "raduino_sbus.h"
SBUS sbus(Serial3);
 
U8GLIB_SH1106_128X64 u8g(46, 45, 44, 12, 11);  
// SW SPI Com: SCK(CLK) = 46, MOSI(DIN) = 45, CS = 44, A0(D/C) = 12, RESET = 11
 
// 모터 변수
//EN1 , IN1a , IN1b : 왼쪽모터   Motor1
//EN2 , IN2a , IN2b : 오른쪽모터 Motor2
int EN1 = 2; int EN2 = 3; int IN1a = 5; int IN1b = 4; int IN2a = 6; int IN2b = 7;
 
// HC-SR04
// trig 53 , echo 50 : 왼쪽 초음파센서
// trig 8  , echo 10 : 오른쪽 초음파센서
int trig[2] = {53,8};   int echo[2] = {50,10};
int distance[2]; int Tdis[2];
 
// 적외선센서 GP2Y0A41SK0F
int IR_Sensor = A0; int IRDistance;
float Voltage;
 
void draw() {
 u8g.setRot180();
  u8g.setFont(u8g_font_unifont);
  u8g.drawStr(50,25, "SBUS");
  u8g.drawStr(55,45, "Car ");
}
 
long cur_goodFrames, old_goodFrames;
 
// this is timer2, which triggers ever 1ms and processes the incoming SBUS datastream
ISR(TIMER2_COMPA_vect)
{
  sbus.process(1);
}
 
void Motor1(int pwm){  
  if(pwm>=0) {
    digitalWrite(IN1a, HIGH);    
    digitalWrite(IN1b, LOW);    
  }
  else {
    digitalWrite(IN1a,LOW);
    digitalWrite(IN1b,HIGH);
  }
  analogWrite(EN1, abs(pwm));
}
 
void Motor2(int pwm){  
  if(pwm>=0) {
    digitalWrite(IN2a, HIGH);    
    digitalWrite(IN2b, LOW);    
  }
  else {
    digitalWrite(IN2a,LOW);
    digitalWrite(IN2b,HIGH);
  }
  analogWrite(EN2, abs(pwm));
}
 
void setup() {
  
  // picture loop
  u8g.firstPage();
  do {
    draw();
  } while(u8g.nextPage());
  
  sbus.begin();
  sbus.SBUS_OUT= true; 
  
  Serial.begin(115200);  
  
  int i;
  
  for(i=0; i<2; i++){
    pinMode(trig[i], OUTPUT);  pinMode(echo[i], INPUT);
  }
  
  for(i = 2; i<=7;i++)  pinMode(i,OUTPUT);
}
 
float us_op(int num)
{
  float Dis=0;
  digitalWrite(trig[num], HIGH);                                
  delayMicroseconds(10);                                       
  digitalWrite(trig[num], LOW);                                  
  distance[num] = pulseIn(echo[num], HIGH, 5000L) * 17/1000; 
  
  Dis = distance[num];
  if(Dis<=0) Dis=0;
  if(Dis>20) Dis=0;
  distance[num]=0;  
  return Dis;
}
 
void loop() 
{
  
  Voltage = analogRead(IR_Sensor)*0.0048828125;  // value from IR_Sensor * (5/1024)
  IRDistance = 13*pow(Voltage, -1); // worked out from datasheet graph
  if(IRDistance > 10) IRDistance = 0;  
  
  int R_Side  = map(sbus.getChannel(1), 310, 1690, -200, 200); 
  int R_Front = map(sbus.getChannel(3), 1690, 310, 200, -200);
 
  if ((R_Front == -202) || (R_Front == 300) || (R_Front == -289)) // 수신기가 OFF
  { 
    Motor1(0);    
    Motor2(0);
  }
 
  else
  {
    Motor1(R_Front+R_Side-int(1500/us_op(1))-int(1500/IRDistance));    
    Motor2(R_Front-R_Side-int(1500/us_op(0))-int(1500/IRDistance));
  }
 
  Serial.print(sbus.getChannel(1));
  Serial.print("   |   ");
  Serial.print(R_Side);
  Serial.print("   |   ");
  Serial.print(sbus.getChannel(2));
  Serial.print("   |   ");
  Serial.print(sbus.getChannel(3));
  Serial.print("   |   ");
  Serial.print(R_Front);
  Serial.print("   |   ");
  Serial.println(sbus.getChannel(4));
 
}

▼ 원격제어 로봇 - SBUS 이용 소스코드 다운로드 

Sbus_car_1023.zip