Ассистент

ARDUINO: Энциклопедия АРДУИНО: Декодер шины I-BUS


Декодер шины I-BUS



В проекте приведен скетч декодера шины I-BUS


#include <string.h>
#define IBUS_BUFFSIZE 32
#define IBUS_MAXCHANNELS 10

#define THROT_CHANNEL 2
#define RUDDER_CHANNEL 3
#define ARM_CHANNEL 4

#define LEFT_PWM_PIN 5
#define RIGHT_PWM_PIN 6
#define LEFT_DIR_PIN 7
#define RIGHT_DIR_PIN 8
#define MANUAL_LED_PIN 13

static uint8_t ibusIndex = 0;
static uint8_t ibus[IBUS_BUFFSIZE] ={0};
static uint16_t rcValue[IBUS_MAXCHANNELS];

boolean rxFrameDone;

int16_t ThrotLeft = 0;
int16_t ThrotRight = 0;
int16_t DirLeft = 1;
int16_t DirRight = 1;


void setup() {
Serial.begin(115200);

pinMode (MANUAL_LED_PIN, OUTPUT);
pinMode (LEFT_PWM_PIN, OUTPUT);
pinMode (RIGHT_PWM_PIN, OUTPUT);
pinMode (LEFT_DIR_PIN, OUTPUT);
pinMode (RIGHT_DIR_PIN, OUTPUT);

digitalWrite(MANUAL_LED_PIN, LOW);
Serial.println("Setup done!");

// Конец инициализации
}



void loop() {
  readRx();
  if(rxFrameDone)
  {
    if(rcValue[ARM_CHANNEL] == 2000)
    {
      digitalWrite(MANUAL_LED_PIN, HIGH);
      UpdateMotors();
          }
          else
          {
            // disarmed from rx
            digitalWrite(MANUAL_LED_PIN, LOW);

            //
            ThrotLeft = 0;
            ThrotRight = 0;
            analogWrite(LEFT_PWM_PIN, ThrotLeft);
            analogWrite(RIGHT_PWM_PIN, ThrotRight);
            DirLeft = 1;
            DirRight = 1;
          }
     }
}

void UpdateMotors()
{
  double tmpThrotRight = 0.00;
  double tmpThrotLeft = 0.00;

  double Throttle = (double)rcValue[THROT_CHANNEL];
  double Rudder = (double)rcValue[RUDDER_CHANNEL];

  tmpThrotLeft = ((((Throttle - 1000) + (Rudder - 1000))/2000)*510) - 255;
  tmpThrotRight = ((((Throttle - 1000) + (2000 - Rudder))/2000)*510) - 255;
  ThrotLeft = (int)tmpThrotLeft;
  ThrotRight = (int)tmpThrotRight;
  digitalWrite(LEFT_DIR_PIN, DetermineMotorDirection(ThrotLeft));
  digitalWrite(RIGHT_DIR_PIN, DetermineMotorDirection(ThrotRight));

  if(ThrotLeft < 0) ThrotLeft = ThrotLeft * -1;
  if(ThrotRight < 0) ThrotRight = ThrotRight * -1;
  analogWrite(LEFT_PWM_PIN, ThrotLeft);
  analogWrite(RIGHT_PWM_PIN, ThrotRight);
 }


int DetermineMotorDirection(int Throttle)
{
  if(Throttle > 0) return HIGH;
  return LOW;
}


void readRx()
{
  rxFrameDone - false;
  if(Serial.available())
  {
    uint8_t val = Serial.read();
    //Lock for 0x2040 as start of packet
    if(ibusIndex == 0 && val != 0x20)
    {
      ibusIndex = 0;
      return;
    }
    if(ibusIndex == 1 && val != 0x40)
    {
      ibusIndex = 0;
      return;
    }
    if (ibusIndex == IBUS_BUFFSIZE)
    {
      ibusIndex = 0;
      int high = 3;
      int low = 2;
      for(int i = 0; i < IBUS_MAXCHANNELS; i++)
      {
        //left shift away the first 8 bits of the first byte and add the whole value of the previous one
        rcValue[i] = (ibus[high] << 8) + ibus[low];
        Serial.print(rcValue[i]);
        Serial.print("     ");
        high += 2;
        low += 2;
      }
        Serial.println();
        rxFrameDone = true;
        return;
      }
      else
      {
        ibus[ibusIndex] = val;
        ibusIndex ++;
      }
     }
    }



 

1