76 lines
1.2 KiB
C++
76 lines
1.2 KiB
C++
//Import
|
|
#include "GY_85.h"
|
|
#include <Wire.h>
|
|
|
|
|
|
GY_85 GY85; //create the object
|
|
|
|
int ax_, ay_, az_;
|
|
int ax, ay, az;
|
|
int cx, cy, cz;
|
|
float gx, gy, gz, gt;
|
|
float acc_pitch;
|
|
|
|
int i, modifier;
|
|
|
|
int m1_in1 = 33;
|
|
int m1_in2 = 25;
|
|
int m2_in1 = 13;
|
|
int m2_in2 = 14;
|
|
|
|
void setup() {
|
|
//Initialize serial
|
|
Serial.begin(57600);
|
|
delay(10);
|
|
|
|
//Initialice I2C
|
|
// Wire.begin(SCL,SDA);
|
|
Wire.begin(26, 27);
|
|
delay(10);
|
|
|
|
//Initialize IMU
|
|
GY85.init();
|
|
delay(10);
|
|
|
|
// Initialize PWM channels
|
|
// channels 0-15, resolution 1-16 bits, freq limits depend on resolution
|
|
// ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
|
|
|
|
ledcAttachPin(m1_in1, 1);
|
|
ledcAttachPin(m1_in2, 2);
|
|
ledcAttachPin(m2_in1, 3);
|
|
ledcAttachPin(m2_in2, 4);
|
|
|
|
ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution
|
|
ledcSetup(2, 12000, 12);
|
|
ledcSetup(3, 12000, 8);
|
|
ledcSetup(4, 12000, 8);
|
|
|
|
i = 0;
|
|
modifier = 2;
|
|
}
|
|
|
|
void loop() {
|
|
//Sense
|
|
readIMU();
|
|
|
|
|
|
//Think
|
|
|
|
|
|
//Act
|
|
motorControl();
|
|
|
|
i=i+modifier;
|
|
|
|
if (i>=4096){
|
|
modifier = -2;
|
|
}
|
|
else if (i<=0){
|
|
modifier = 2;
|
|
}
|
|
|
|
//Delay
|
|
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
|
|
}
|