144 lines
2.6 KiB
C++
144 lines
2.6 KiB
C++
//Import
|
|
#include "GY_85.h"
|
|
#include <Wire.h>
|
|
|
|
GY_85 GY85; //create the object
|
|
|
|
//System variables
|
|
unsigned long tNow = micros();
|
|
|
|
//Motor Control variables
|
|
const byte m1_in1 = 18;
|
|
const byte m1_in2 = 19;
|
|
//int m2_in1 = 16;
|
|
//int m2_in2 = 17;
|
|
|
|
//Encoders variables
|
|
long int m1Raw,m1RawLast;
|
|
const byte pin_m1_A = 34;
|
|
const byte pin_m1_B = 35;
|
|
volatile bool A_state;
|
|
volatile bool B_state;
|
|
|
|
//Interrupt routines
|
|
// CW = INCREASE
|
|
// CCW = DECREASE
|
|
|
|
void IRAM_ATTR m1_A_changed() {
|
|
A_state = digitalRead(pin_m1_A);
|
|
B_state = digitalRead(pin_m1_B);
|
|
|
|
//Rising
|
|
if (A_state == HIGH) {
|
|
if (B_state == HIGH) {
|
|
m1Raw = m1Raw - 1;
|
|
}
|
|
else if (B_state == LOW) {
|
|
m1Raw = m1Raw + 1;
|
|
}
|
|
}
|
|
|
|
//Falling
|
|
else if (A_state == LOW) {
|
|
if (B_state == HIGH) {
|
|
m1Raw = m1Raw + 1;
|
|
}
|
|
else if (B_state == LOW) {
|
|
m1Raw = m1Raw - 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
void IRAM_ATTR m1_B_changed() {
|
|
A_state = digitalRead(pin_m1_A);
|
|
B_state = digitalRead(pin_m1_B);
|
|
|
|
//Rising
|
|
if (B_state == HIGH) {
|
|
if (A_state == HIGH) {
|
|
m1Raw = m1Raw + 1;
|
|
}
|
|
else if (A_state == LOW) {
|
|
m1Raw = m1Raw - 1;
|
|
}
|
|
}
|
|
|
|
//Falling
|
|
else if (B_state == LOW) {
|
|
if (A_state == HIGH) {
|
|
m1Raw = m1Raw - 1;
|
|
}
|
|
else if (A_state == LOW) {
|
|
m1Raw = m1Raw + 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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 Interrupts
|
|
pinMode(pin_m1_A, INPUT_PULLUP);
|
|
pinMode(pin_m1_B, INPUT_PULLUP);
|
|
attachInterrupt(digitalPinToInterrupt(pin_m1_A), m1_A_changed, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(pin_m1_B), m1_B_changed, CHANGE);
|
|
|
|
//Initialize Encoders
|
|
m1Raw = 0;
|
|
m1RawLast = 100;
|
|
|
|
// 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);
|
|
|
|
Serial.println("Reference,Actual,SpeedCommand");
|
|
|
|
}
|
|
|
|
void loop() {
|
|
//Update system variables
|
|
tNow = micros();
|
|
|
|
//// //Only print encoder value if value changed since last print
|
|
// if (m1Raw != m1RawLast) {
|
|
// Serial.println(m1Raw);
|
|
// m1RawLast = m1Raw;
|
|
// }
|
|
|
|
//Sense
|
|
readIMU();
|
|
|
|
|
|
//Think
|
|
|
|
|
|
//Act
|
|
motorControl();
|
|
|
|
|
|
//Delay
|
|
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
|
|
}
|