46 lines
697 B
C++
46 lines
697 B
C++
//Import
|
|
#include "GY_85.h"
|
|
#include <Wire.h>
|
|
|
|
//Define L298N pin mappings
|
|
const int IN1 = 3;
|
|
const int IN2 = 2;
|
|
const int IN3 = 5;
|
|
const int IN4 = 4;
|
|
#define LOW 0
|
|
|
|
GY_85 GY85; //create the object
|
|
|
|
int ax_, ay_, az_;
|
|
int ax, ay, az;
|
|
int cx, cy, cz;
|
|
float gx, gy, gz, gt;
|
|
|
|
|
|
|
|
void setup() {
|
|
|
|
// Wire.begin(SCL,SDA);
|
|
Wire.begin(26, 27);
|
|
delay(10);
|
|
Serial.begin(57600);
|
|
delay(10);
|
|
GY85.init();
|
|
delay(10);
|
|
|
|
//Assign H bridge Pins
|
|
pinMode(IN1, OUTPUT);
|
|
pinMode(IN2, OUTPUT);
|
|
pinMode(IN3, OUTPUT);
|
|
pinMode(IN4, OUTPUT);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
readIMU();
|
|
motorControl();
|
|
|
|
//Delay
|
|
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
|
|
}
|