Using reading from Gyro(IMU) to control direction of motor(bi-directional)

Hi there guys trying to make the motor turn in both directions using the output from the reading of the Gyrometer(IMU) I’m using.
Apparently the serial monitor does not show anything and motor just won’t work too. Anyone here specialized in these area able to help me ?
Kinda rush as deadline is near :frowning:

//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
int motor1pin = 9;
int motor2pin = 10;
int enablepin = 12;
int LED_green = 8;
int LED_red = 7;
#define level 0
#define gX A0
#define aX A3
#define aY A4
#define aZ A5

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;

int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;


//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

int inputAccY[100];//y-axis
long resultAccY;

int inputAccZ[100];//z-axis
long resultAccZ;

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    inputGyroX[i] = analogRead(gX);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroX += inputGyroX[i];
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}
 
int calibrateAccY()
{
  for(int i=0;i<100;i++)
  {
    inputAccY[i] = analogRead(aY);
  }
  for(int i=0;i<100;i++)
  {
    resultAccY += inputAccY[i];
  }
  resultAccY = resultAccY/100;
  return resultAccY;
}

int calibrateAccZ()
{
  for(int i=0;i<100;i++)
  {
    inputAccZ[i] = analogRead(aZ);
  }
  for(int i=0;i<100;i++)
  {
    resultAccZ += inputAccZ[i];
  }
  resultAccZ = resultAccZ/100;
  return resultAccZ;
}
//compensation
int negative_compensation()
{
   while(analogRead(gyroXangle) > level)
    {
      digitalWrite(LED_red,LOW);
      digitalWrite(LED_green,HIGH);
      analogWrite (motor1pin,255);
      analogWrite (motor2pin,0);
      //break;
       if(analogRead(gyroXangle)>(level-2) && analogRead(gyroXangle)<(level+2))
      {
        digitalWrite (motor1pin,LOW);
        digitalWrite (motor2pin,LOW);
      }
    }
}

int positive_compensation()
{
  while(analogRead(gyroXangle) < level)
    {
      digitalWrite(LED_red,HIGH);
      digitalWrite(LED_green,LOW);
      analogWrite (motor1pin,0);
      analogWrite (motor2pin,255);
        if(analogRead(gyroXangle)>(level-2) && analogRead(gyroXangle)<(level+2))
        {
          digitalWrite (motor1pin,LOW);
          digitalWrite (motor2pin,LOW);
        }
    }
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.3; 
    float Q_gyroX   =  0.002;  
    float R_angleX  =  0.03;  

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;	
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
    
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
    
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
    
    return x_angle;
  }

 // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.3; 
    float Q_gyroY   =  0.002;  
    float R_angleY  =  0.03;  

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;	
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
    
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
    
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
    
    return y_angle;
  }

 //print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
  
  //Serial.print(accXangle);Serial.print("\t");
  //Serial.print(accYangle);Serial.print("\t");  
  
  Serial.print(compAngleX);Serial.print("\t");  
  Serial.print(compAngleY); Serial.print("\t"); 
  
  Serial.print(xAngle);Serial.print("\t");
  Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
} 



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
  
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();
  
  pinMode (motor1pin,OUTPUT);
  pinMode (motor2pin,OUTPUT);
  pinMode (enablepin,OUTPUT);
  pinMode (LED_green, OUTPUT);
  pinMode (LED_red, OUTPUT);
  
  digitalWrite (enablepin,HIGH);
  
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter

  
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accYadc = analogRead(aY);
  accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accZadc = analogRead(aZ);
  accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  accZval++;//1g in horizontal position
  
  R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  accZangle = acos(accZval/R)*RAD_TO_DEG;
  processing(); 
  
  dtime = millis()-timer;
  timer = millis();
  
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);


  if(analogRead(gyro) > level)
  {
    negative_compensation();
  }
  else if(analogRead(gyro) < level)
  {
    positive_compensation();
  }
 
 
}
int inputGyroX[100];//x-axis
int inputAccX[100];//x-axis
int inputAccY[100];//y-axis
int inputAccZ[100];//z-axis

That’s 800 bytes of memory gone, right there. The UNO and other 328 based devices only have 2000 bytes of memory.

#define RAD_TO_DEG 57.295779513082320876798154814105

Doubles and floats on the Arduino have 6 to 7 significant digits. Typing 37 digits after the decimal point won’t make it more accurate.

  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter

Mixed mode arithmetic is rarely a good idea. float, float, integer, integer. The division by an integer should be avoided. Cast it to a float, or add .0 on the end to make it a float.

Of course, you tested that you can actually read from the device before adding a bazillion lines of code to manipulate what you read, right?

I’d say that there is about a 99.9999999999999999999999% chance that you don’t have near enough memory for all this code.