MPU6050 und bluetoothverbindung über HC05

[inka]

also ohne mpu verwende ich die kombination sehr oft, hier z.b. werden an der gleichen hardware die radencoder ausgelesen und ausgegeben. Funktioniert mit serial, wie auch mit serial1. Gerade noch einmal getestet...

#include "Arduino.h"
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#define encoder_A 2
#define encoder_B 3

uint8_t i, speed, s_speed, m_speed, b_speed;

int wert_encoder_[] = {1, 2};
int mehrfach_enc_zaehler_[] = {1, 2};
int summe_encoder_[] = {1, 2};
int vorwaerts_[] = {1, 2};

unsigned long dauer_servo, dauer_fahrt, currentMillis, previousMillis;

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *motor_A = AFMS.getMotor(3);//
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);//
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

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

  lcd.begin(16, 2);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.setBacklight(HIGH);
  lcd.print("alle_mot_enc_1");
  lcd.setCursor(0, 1);
  lcd.print("ausl_li_re");
  delay(2000);
  lcd.clear();

  pinMode(encoder_A, INPUT_PULLUP);
  pinMode(encoder_B, INPUT_PULLUP);

s_speed = 100;
  m_speed = 150;
  b_speed = 200;
  speed = 150;

for (i = 1; i < 2; i++)
{
    vorwaerts_[i] = 1;
    wert_encoder_[i] = 0;
    mehrfach_enc_zaehler_[i] = 0;
  }

  AFMS.begin();
}

void loop()
{
if (vorwaerts_[1] == 1) rueckwaerts();
  else vorwaerts();
}

void motor_stop(void)
{
  motor_A->setSpeed(0);
  motor_B->setSpeed(0);
  motor_A->run(BRAKE);
  motor_B->run(BRAKE);
  motor_A->run(RELEASE);
  motor_B->run(RELEASE);
  delay(250);
  summe_encoder_[1] = 0;
}


void rueckwaerts(void)
{
  motor_A->setSpeed(speed);
  motor_B->setSpeed(speed);
  motor_A->run(BACKWARD);
  motor_B->run(BACKWARD);
  encoder_auslesen_A();

  if (summe_encoder_[1] >= 100)

  {
    summe_encoder_[1] = 0;
    vorwaerts_[1] = 0;
    motor_stop();
  }
}


void vorwaerts(void)
{
  motor_A->setSpeed(speed);
  motor_B->setSpeed(speed);
  motor_A->run(FORWARD);
  motor_B->run(FORWARD);
  encoder_auslesen_A();

  if (summe_encoder_[1] <= -100) //>= 100)
  {
    summe_encoder_[1] = 0;
    vorwaerts_[1] = 1;
    motor_stop();
  }
}


void encoder_auslesen_A(void)
{
  wert_encoder_[1] = digitalRead(encoder_A);
  if (vorwaerts_[1] == 1)
  {
    if (wert_encoder_[1] == 0 && mehrfach_enc_zaehler_[1] == 0)
    {
      mehrfach_enc_zaehler_[1] = 1;
    }
    else if (wert_encoder_[1] == 1 && mehrfach_enc_zaehler_[1] == 1)
    {
      mehrfach_enc_zaehler_[1] = 0;
      summe_encoder_[1] = (summe_encoder_[1] + wert_encoder_[1]);
      Serial.print(wert_encoder_[1]);
      Serial.print("    ");
      Serial.println(summe_encoder_[1]);
      Serial1.print(wert_encoder_[1]);
      Serial1.print("    ");
      Serial1.println(summe_encoder_[1]);
    }
  }
  else
  {
    if (wert_encoder_[1] == 0 && mehrfach_enc_zaehler_[1] == 0)
    {
      mehrfach_enc_zaehler_[1] = 1;
    }
    else if (wert_encoder_[1] == 1 && mehrfach_enc_zaehler_[1] == 1)
    {
      mehrfach_enc_zaehler_[1] = 0;
      summe_encoder_[1] = (summe_encoder_[1] - wert_encoder_[1]); //+
      Serial.print(wert_encoder_[1]);
      Serial.print("    ");
      Serial.println(summe_encoder_[1]);
      Serial1.print(wert_encoder_[1]);
      Serial1.print("    ");
      Serial1.println(summe_encoder_[1]);
    }
  }
}