trueopenvr / trueopenvr-drivers Goto Github PK

#include <Arduino.h>
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include <Arduino_LSM6DS3.h>
#include "MadgwickAHRS.h"

// initialize a Madgwick filter:
Madgwick filter;

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

//Controller TrueOpenVR

#define GRIP_BTN  0x0001
#define THUMB_BTN 0x0002
#define A_BTN   0x0004
#define B_BTN   0x0008
#define MENU_BTN  0x0010
#define SYS_BTN   0x0020

//Digital pins number / íîìåðà öèôðîâûõ ïèíîâ
const byte TriggerBtnPin = 3;
const byte GripBtnPin = 4;
const byte ThumbStickBtnPin = 5;
const byte MenuBtnPin = 6;
const byte SystemBtnPin = 7;

//Button for emulating stick movement
const byte UpStickPin = 7;
const byte LeftStickPin = 8;
const byte RightStickPin = 9;
const byte DownStickPin = 10;

float ctrl[12];


// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    if (!IMU.begin()) {
      Serial.println("Failed to initialize IMU!");
      while (1);
    }

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    filter.begin(104.00);

    //Setup pins
    pinMode(TriggerBtnPin, INPUT_PULLUP);
    pinMode(GripBtnPin, INPUT_PULLUP);
    pinMode(ThumbStickBtnPin, INPUT_PULLUP);
    pinMode(SystemBtnPin, INPUT_PULLUP);
    pinMode(MenuBtnPin, INPUT_PULLUP);

    //Button for emulating stick movement
    //Êíîïêè äëÿ ýìóëÿöèè äâèæåíèÿ ñòèêîâ
    pinMode(UpStickPin, INPUT_PULLUP);
    pinMode(LeftStickPin, INPUT_PULLUP);
    pinMode(RightStickPin, INPUT_PULLUP);
    pinMode(DownStickPin, INPUT_PULLUP);

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
    // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.
    while (!Serial);
    // initialize device
    //Serial.println(F("Initializing I2C devices..."));
    pinMode(INTERRUPT_PIN, INPUT);
    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

float xg, yg, zg;
float xa, ya, za;
float pitch, roll, yaw;

void loop() {
  if (IMU.gyroscopeAvailable() && IMU.accelerationAvailable()) {
    IMU.readGyroscope(xg, yg, zg);
    IMU.readAcceleration(xa, ya, za);
    // update the filter, which computes orientation:
    filter.updateIMU(xg, yg, zg, xa, ya, za);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    yaw = filter.getYaw();
  }


  //Set pos for testing
  ctrl[0] = 0; ////x -0.1
  ctrl[1] = 0;  //y -0.3
  ctrl[2] = 0; //y -0.1
  //If X,Y,Z positions are zeros, then the position is calculated based on IMUs
  //Åñëè X,Y,Z ïîçèöèè ïî íóëÿì çíà÷èò ïîçèöèÿ ðàññ÷èòûâàåòñÿ íà îñíîâå IMUs
  
  ctrl[3] = yaw + 180.0;
  ctrl[4] = roll + 180.0;
  ctrl[5] = pitch + 180.0;

  //Serial.print(yaw);
  //Serial.print("\t");
  //Serial.print(pitch);
  //Serial.print("\t");
  //Serial.println(roll);

  //Position shoulder
  ctrl[6] = 0;
  ctrl[7] = 0;

  //Buttons
  ctrl[8] = 0; //Buttons
  ctrl[9] = 0; //Trigger
  ctrl[10] = 0; //ThumbX
  ctrl[11] = 0; //ThumbY

  //Checking press buttons 

  int CtrlButtons = 0;
  if (digitalRead(GripBtnPin) == LOW)
    CtrlButtons |= GRIP_BTN;

  if (digitalRead(ThumbStickBtnPin) == LOW)
    CtrlButtons |= THUMB_BTN; 

  if (digitalRead(MenuBtnPin) == LOW)
    CtrlButtons |= MENU_BTN;

  if (digitalRead(SystemBtnPin) == LOW)
    CtrlButtons |= SYS_BTN;

  //Buttons
    ctrl[8] = CtrlButtons;

  //Trigger
  if (digitalRead(TriggerBtnPin) == LOW)
    ctrl[9] = 1;

  //Stick emulation
  if (digitalRead(UpStickPin) == LOW)
    ctrl[11] = 1; //Up

  if (digitalRead(LeftStickPin) == LOW)
    ctrl[10] = -1; //Left

  if (digitalRead(RightStickPin) == LOW)
    ctrl[10] = 1; //Right

  if (digitalRead(DownStickPin) == LOW)
    ctrl[11] = -1; //Down

  //Output binary
  if (Serial.availableForWrite() > 0) {
    Serial.write((byte*)&ctrl, sizeof(ctrl));
  }

  //blinkState = !blinkState;
  //digitalWrite(LED_PIN, blinkState);
}

[Main]
ShoulderYaw=10
ShoulderPitch=25
ScalePos=0.005
ScalePosZ=0.010
OffsetPosZ=0.1

[Controller1]
ComPort=7

[Controller2]
ComPort=8