Friday, June 16, 2017

Arduino Nano prototype dual gyro handcontroller 2017

#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

#define LEDG 3
#define LEDB 5
#define LEDR 6

#define LEDR2 9
#define LEDG2 10
#define LEDB2 11


// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
MPU6050 accelgyro2(0x69); // <-- use for AD0 high


////////////////////////////////////////////////////////////////////////

void setup() {
  // put your setup code here, to run once:
pinMode(LEDB,OUTPUT);
pinMode(LEDR,OUTPUT);
pinMode(LEDG,OUTPUT);

digitalWrite(LEDB,LOW);
digitalWrite(LEDG,HIGH);
digitalWrite(LEDR,HIGH);

pinMode(LEDB2,OUTPUT);
pinMode(LEDR2,OUTPUT);
pinMode(LEDG2,OUTPUT);
digitalWrite(LEDR2,HIGH);
digitalWrite(LEDG2,HIGH);
digitalWrite(LEDB2,LOW);


delay(1000);

Fastwire::setup(400, false);
    Serial.begin(115200);

   accelgyro.initialize();
    accelgyro2.initialize();

    if(accelgyro.testConnection()){
      digitalWrite(LEDR2,HIGH);
       digitalWrite(LEDG2,HIGH);
        digitalWrite(LEDB2,LOW);
    }
    else {
      digitalWrite(LEDR2,LOW);
      digitalWrite(LEDG2,HIGH);
      digitalWrite(LEDB2,HIGH);
    }
    if(accelgyro2.testConnection()){
      digitalWrite(LEDR,HIGH);
       digitalWrite(LEDG,HIGH);
        digitalWrite(LEDB,LOW);
    }
    else {
      digitalWrite(LEDR,LOW);
      digitalWrite(LEDG,HIGH);
      digitalWrite(LEDB,HIGH);
    }
}
///////////////////////////////////////////////////////////////////////


int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;
float angle2=0.0;
float angle3=0.0;
float angle4=0.0;



bool tripped=false;
bool flash=true;
unsigned int flash_counter=0,flash_count=0;
///////////////////////////////////////////////////////////////////////////
void loop() {


  flash_counter++;
  if(flash_counter>flash_count)flash_counter=0;
  if(flash_counter>flash_count/2)flash=true;
 else flash=false;


//////////////////////////////////////////////
tripped=false;
  
      state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
          
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
     state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);// 
          
     if(state>=0){
     ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }

  if(!tripped){
          angle1=atan2(ax,-az)*180/M_PI;
          angle2=atan2(ay,-az)*180/M_PI;
  
  //  Serial.print(ax);       Serial.print("\t");
    //    Serial.print(ay);       Serial.print("\t");
   //     Serial.print(az);       Serial.print("\t");
      
  }
       ////////////////////////////////////////////////////////////////////////////////////////////
    //  tripped=false;
  
      state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);// 
     //Serial.print(state);
     
     if(state>=0){
     ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }
     state=I2Cdev::readBytes(0x69, 0x3F, 2,BUFFER);
          if(state>=0){
     az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
      }
     else{
       // Serial.print("FAIL2");
      Fastwire::reset();
      tripped=true;
     }
     state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_XOUT_H,2, BUFFER);// 
          
     if(state>=0){
     ax= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
     }
     else{
       // Serial.print("FAIL1");
        Fastwire::reset();
        tripped=true;
        // Fastwire::setup(400, false);   
     }   


      
       if(!tripped){
          angle3=atan2(ax,-az)*128/M_PI;
          angle4=atan2(ay,-az)*128/M_PI;
             Serial.print(angle1);       Serial.print("\t");
    Serial.print(angle2);       Serial.print("\t");
    Serial.print(angle3);       Serial.print("\t");
    Serial.print(angle4);       Serial.println("");
         
         flash_count=0.6*(abs(180-angle1)+abs(angle3));
          if(flash){
             digitalWrite(LEDB,LOW);
        
          digitalWrite(LEDG,HIGH);
            analogWrite(LEDR,(int)abs(128+angle2*128/180));
          }
          else {
            digitalWrite(LEDB,LOW);
            digitalWrite(LEDR,LOW);
          digitalWrite(LEDG,HIGH);
          }
          if(flash){
              digitalWrite(LEDB2,LOW);
        
          digitalWrite(LEDG2,HIGH);
            analogWrite(LEDR2,(int)abs(128+angle4*128/180));
          }
          else {
            
            digitalWrite(LEDB2,LOW);
            digitalWrite(LEDR2,LOW);
          digitalWrite(LEDG2,HIGH);
          }
          // Serial.println(angle2);
       }
}

No comments:

Post a Comment