Problem using parallel a sensor with I2C and a motor with a Timer (PWM)

Other Hardware Sensors
3

Hi,

the crash looks like there is no longer an output in the Serial Monitor. If i use the programm with low speedValues for the motor (like 30 with a max of 400) the the programm runs and gives the wanted results. If i use higher speedvalues then the output on the Serial Monitor stops after a few lines and it is not longer possible to insert new speedvalues to the programm.

The code is:

#include <Shield2.h>
#include <Wire.h>

Shield md;

#define AccAddress  0x53
#define GyroAddress 0x68

int speedValue=0;   // speedValue for motor
int speedV =0;

int parseInput() // parse input from Serial to a signed number
{
  String inString = "";
  int sign =1;
  int inChar=0;
  do
  {
    inChar = Serial.read();
    if ((char)inChar =='-') 
    {
      sign =-1;
    }
    if (isDigit(inChar)) 
    {
      inString += (char)inChar; 
    } 
    if (inChar == '\n') 
    {
      return sign*inString.toInt(); 
    }
    Serial.println("reading");
  } 
  while (inChar != '\n');
}

void setup()
{
  Wire.begin();
  Serial.begin(115200);
  md.init();
}

void writeTo(int _dev_address, byte address, byte val) {
  Wire.beginTransmission(_dev_address); // start transmission to device
  Wire.write(address);             // send register address
  Wire.write(val);                 // send value to write
  Wire.endTransmission();         // end transmission
}

void loop()
{

  writeTo(AccAddress,0x2D,0x08); // Power CTL : ON
  writeTo(AccAddress,0x31,1); // DataFormat for acc-Sensor  

  Wire.beginTransmission(GyroAddress);
  Wire.write(0x1D);
  Wire.requestFrom(GyroAddress,6);
  byte temp[6];
  int i=0;
  while (Wire.available())
  {
    temp[i++]=Wire.read();
  }
  int tempval1 =  ((temp[0]<<8) |temp[1]);
  int tempval2 = ((temp[2]<<8) |temp[3]);
  int tempval3 = ((temp[4]<<8) |temp[5]);
  Wire.endTransmission();

  Wire.beginTransmission(AccAddress);
  Wire.write(0x32);
  Wire.requestFrom(AccAddress,6);

  i=0;
  while (Wire.available())
  {
    temp[i++]=Wire.read();
  }
  double tempvala1 =  (((temp[1]<<8) |temp[0]));
  double tempvala2 = (((temp[3]<<8) |temp[2]));
  double tempvala3 = (((temp[5]<<8) |temp[4]));
  Wire.endTransmission();


  if (Serial.available() > 0)
  {  
    speedValue = parseInput();
  }
  if (speedValue <-400 || speedValue > 400) 
  {
    speedValue = speedV;
  }
  else speedV = speedValue;
  md.setSpeedv(speedValue);
   
  double a1 = tempvala1*2/65536;
  double a2 = tempvala2*2/65536;
  double a3 = tempvala3*2/65536;
  double a0 = sqrt(a1*a1+a2*a2+a3*a3); 
  Serial.print(a1);
  Serial.print(" | ");
  Serial.print(a2);
  Serial.print(" | ");
  Serial.print(a3);
  Serial.print(" | a: ");
  Serial.println(a0);
}

The Functions from "Shield2.h" are:

void Shield::init()
{
  pinMode(_FF1 ,INPUT);
  pinMode(_FF2 ,INPUT);
  pinMode(_PWM ,OUTPUT);
  pinMode(_DIR ,OUTPUT);
  pinMode(_CS  ,INPUT);
 
  // PWM frequency calculation
  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
  TCCR1A = 0b10100000;
  TCCR1B = 0b00010001;
  ICR1 = 400;
}

void Shield::setSpeedv(int speed)
{
  unsigned char reverse = 0;
  if (speed < 0)
  {
    speed = -speed;  
    reverse = 1;
  }
  if (speed > 400)  
    speed = 400;
  OCR1A = speed;
  if (reverse) { digitalWrite(_DIR,LOW);}
  else { digitalWrite(_DIR,HIGH); }
}

with the pinmapping:

 static const unsigned char _FF1 = 12;
 static const unsigned char _FF2 =  9;
 static const unsigned char _PWM = 11;
 static const unsigned char _DIR = 13;
 static const unsigned char _CS  = A0;

The Sensor is connected to the Pins 20 (SDA) and 21 (SCL) to the 3.3V and to GND.

I hope you have an idea where this problem can come from.