Expanded to handle 2 motors + cleanup of variables
This commit is contained in:
Stedd
parent
144b63d9f8
commit
5f57c6cb5c
21
Main/IMU.ino
21
Main/IMU.ino
|
|
@ -1,5 +1,4 @@
|
|||
//IMU variables
|
||||
|
||||
int ax_, ay_, az_;
|
||||
int ax, ay, az;
|
||||
int cx, cy, cz;
|
||||
|
|
@ -7,18 +6,18 @@ float gx, gy, gz, gt;
|
|||
float acc_pitch;
|
||||
|
||||
void readIMU() {
|
||||
ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() );
|
||||
ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() );
|
||||
az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() );
|
||||
ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() );
|
||||
ay_ = IMU.accelerometer_y( IMU.readFromAccelerometer() );
|
||||
az_ = IMU.accelerometer_z( IMU.readFromAccelerometer() );
|
||||
|
||||
// cx = GY85.compass_x( GY85.readFromCompass() );
|
||||
// cy = GY85.compass_y( GY85.readFromCompass() );
|
||||
// cz = GY85.compass_z( GY85.readFromCompass() );
|
||||
// cx = IMU.compass_x( IMU.readFromCompass() );
|
||||
// cy = IMU.compass_y( IMU.readFromCompass() );
|
||||
// cz = IMU.compass_z( IMU.readFromCompass() );
|
||||
|
||||
gx = GY85.gyro_x( GY85.readGyro() );
|
||||
gy = GY85.gyro_y( GY85.readGyro() );
|
||||
gz = GY85.gyro_z( GY85.readGyro() );
|
||||
// gt = GY85.temp ( GY85.readGyro() );
|
||||
gx = IMU.gyro_x( IMU.readGyro() );
|
||||
gy = IMU.gyro_y( IMU.readGyro() );
|
||||
gz = IMU.gyro_z( IMU.readGyro() );
|
||||
// gt = IMU.temp ( IMU.readGyro() );
|
||||
|
||||
|
||||
//For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
|
||||
|
|
|
|||
175
Main/Main.ino
175
Main/Main.ino
|
|
@ -2,78 +2,148 @@
|
|||
#include "GY_85.h"
|
||||
#include <Wire.h>
|
||||
|
||||
GY_85 GY85; //create the object
|
||||
|
||||
//Declare library objects
|
||||
GY_85 IMU;
|
||||
|
||||
|
||||
//GPIO PIN MAPPING
|
||||
const byte M1_ENC_A = 34;
|
||||
const byte M1_ENC_B = 35;
|
||||
const byte M2_ENC_A = 32;
|
||||
const byte M2_ENC_B = 33;
|
||||
const byte M1_A = 18;
|
||||
const byte M1_B = 19;
|
||||
const byte M2_A = 16;
|
||||
const byte M2_B = 17;
|
||||
const byte IMU_I2C_SCL = 26;
|
||||
const byte IMU_I2C_SDA = 27;
|
||||
|
||||
|
||||
//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;
|
||||
|
||||
//Motor variables
|
||||
const int PWM_CYCLE = 12000;
|
||||
const byte PWM_RESOLUTION = 12;
|
||||
|
||||
//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;
|
||||
long int m1Raw, m1RawLast;
|
||||
long int m2Raw, m2RawLast;
|
||||
volatile bool M1_A_state;
|
||||
volatile bool M1_B_state;
|
||||
volatile bool M2_A_state;
|
||||
volatile bool M2_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);
|
||||
M1_A_state = digitalRead(M1_ENC_A);
|
||||
M1_B_state = digitalRead(M1_ENC_B);
|
||||
|
||||
//Rising
|
||||
if (A_state == HIGH) {
|
||||
if (B_state == HIGH) {
|
||||
if (M1_A_state == HIGH) {
|
||||
if (M1_B_state == HIGH) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
else if (B_state == LOW) {
|
||||
else if (M1_B_state == LOW) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
}
|
||||
|
||||
//Falling
|
||||
else if (A_state == LOW) {
|
||||
if (B_state == HIGH) {
|
||||
else if (M1_A_state == LOW) {
|
||||
if (M1_B_state == HIGH) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
else if (B_state == LOW) {
|
||||
else if (M1_B_state == LOW) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void IRAM_ATTR m1_B_changed() {
|
||||
A_state = digitalRead(pin_m1_A);
|
||||
B_state = digitalRead(pin_m1_B);
|
||||
M1_A_state = digitalRead(M1_ENC_A);
|
||||
M1_B_state = digitalRead(M1_ENC_B);
|
||||
|
||||
//Rising
|
||||
if (B_state == HIGH) {
|
||||
if (A_state == HIGH) {
|
||||
if (M1_B_state == HIGH) {
|
||||
if (M1_A_state == HIGH) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
else if (A_state == LOW) {
|
||||
else if (M1_A_state == LOW) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
}
|
||||
|
||||
//Falling
|
||||
else if (B_state == LOW) {
|
||||
if (A_state == HIGH) {
|
||||
else if (M1_B_state == LOW) {
|
||||
if (M1_A_state == HIGH) {
|
||||
m1Raw = m1Raw - 1;
|
||||
}
|
||||
else if (A_state == LOW) {
|
||||
else if (M1_A_state == LOW) {
|
||||
m1Raw = m1Raw + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IRAM_ATTR m2_A_changed() {
|
||||
M2_A_state = digitalRead(M2_ENC_A);
|
||||
M2_B_state = digitalRead(M2_ENC_B);
|
||||
|
||||
//Rising
|
||||
if (M2_A_state == HIGH) {
|
||||
if (M2_B_state == HIGH) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
else if (M2_B_state == LOW) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
}
|
||||
|
||||
//Falling
|
||||
else if (M2_A_state == LOW) {
|
||||
if (M2_B_state == HIGH) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
else if (M2_B_state == LOW) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void IRAM_ATTR m2_B_changed() {
|
||||
M2_A_state = digitalRead(M2_ENC_A);
|
||||
M2_B_state = digitalRead(M2_ENC_B);
|
||||
|
||||
//Rising
|
||||
if (M2_B_state == HIGH) {
|
||||
if (M2_A_state == HIGH) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
else if (M2_A_state == LOW) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
}
|
||||
|
||||
//Falling
|
||||
else if (M2_B_state == LOW) {
|
||||
if (M2_A_state == HIGH) {
|
||||
m2Raw = m2Raw - 1;
|
||||
}
|
||||
else if (M2_A_state == LOW) {
|
||||
m2Raw = m2Raw + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void setup() {
|
||||
//Initialize serial
|
||||
|
|
@ -81,39 +151,44 @@ void setup() {
|
|||
delay(10);
|
||||
|
||||
//Initialice I2C
|
||||
// Wire.begin(SCL,SDA);
|
||||
Wire.begin(26, 27);
|
||||
Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
|
||||
delay(10);
|
||||
|
||||
//Initialize IMU
|
||||
GY85.init();
|
||||
IMU.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 encoder interrupts
|
||||
pinMode(M1_ENC_A, INPUT_PULLUP);
|
||||
pinMode(M1_ENC_B, INPUT_PULLUP);
|
||||
pinMode(M2_ENC_A, INPUT_PULLUP);
|
||||
pinMode(M2_ENC_B, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
|
||||
|
||||
//Initialize Encoders
|
||||
//Initialize encoders
|
||||
m1Raw = 0;
|
||||
m1RawLast = 100;
|
||||
m2Raw = 0;
|
||||
m2RawLast = 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);
|
||||
ledcAttachPin(M1_A, 1);
|
||||
ledcAttachPin(M1_B, 2);
|
||||
ledcAttachPin(M2_A, 3);
|
||||
ledcAttachPin(M2_B, 4);
|
||||
|
||||
ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution
|
||||
ledcSetup(2, 12000, 12);
|
||||
// ledcSetup(3, 12000, 8);
|
||||
// ledcSetup(4, 12000, 8);
|
||||
ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION);
|
||||
ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION);
|
||||
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
|
||||
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
|
||||
|
||||
Serial.println("Reference,Actual,SpeedCommand");
|
||||
Serial.println("Reference,Actual,SpeedCommand");
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -121,11 +196,11 @@ 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;
|
||||
// }
|
||||
//// //Only print encoder value if value changed since last print
|
||||
// if (m1Raw != m1RawLast) {
|
||||
// Serial.println(m1Raw);
|
||||
// m1RawLast = m1Raw;
|
||||
// }
|
||||
|
||||
//Sense
|
||||
readIMU();
|
||||
|
|
|
|||
|
|
@ -2,39 +2,42 @@
|
|||
const float wheel_diameter = 0.067708;
|
||||
const float pulses_per_turn = 1320.0;
|
||||
//Tuning
|
||||
const float K = 2;
|
||||
const float I = 10;
|
||||
const float K = 2.0;
|
||||
const float I = 10.0;
|
||||
|
||||
//Help variables
|
||||
int dEnc, dT;
|
||||
float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered;
|
||||
unsigned long tLast;
|
||||
|
||||
float speed_setp,referenceSpeed,actualSpeed;
|
||||
float speed_setp, referenceSpeed, actualSpeed;
|
||||
float error, iError;
|
||||
int speedCMD;
|
||||
|
||||
|
||||
void motorControl() {
|
||||
//Motor 1
|
||||
speed_setp = map(acc_pitch, -90.0, 90.0, -4096, 4096);
|
||||
referenceSpeed = speed_setp;
|
||||
actualSpeed = lin_vel*4096;
|
||||
error = referenceSpeed - actualSpeed;
|
||||
iError = iError + (error*dT*pow(10,-6)*I);
|
||||
speedCMD = round((error*K)+iError);
|
||||
|
||||
// Saturation
|
||||
if(speedCMD>4096){
|
||||
//Speed Controller
|
||||
referenceSpeed = acc_pitch * ((4096.0) / (90.0));
|
||||
actualSpeed = lin_vel * 4096.0;
|
||||
error = referenceSpeed - actualSpeed;
|
||||
iError = iError + (error * dT * pow(10, -6) * I);
|
||||
speedCMD = round((error * K) + iError);
|
||||
|
||||
|
||||
// Speed command saturation
|
||||
if (speedCMD > 4096) {
|
||||
speedCMD = 4096;
|
||||
}
|
||||
else if(speedCMD < -4096){
|
||||
else if (speedCMD < -4096) {
|
||||
speedCMD = -4096;
|
||||
}
|
||||
else{
|
||||
else {
|
||||
speedCMD = speedCMD;
|
||||
}
|
||||
|
||||
|
||||
//Motor 1 Control
|
||||
if (speedCMD > 0) {
|
||||
ledcWrite(1, 0);
|
||||
ledcWrite(2, speedCMD);
|
||||
|
|
@ -56,27 +59,27 @@ void motorControl() {
|
|||
// ledcWrite(3, 255);
|
||||
// ledcWrite(4, 255);
|
||||
|
||||
//Calculate speed from encoders
|
||||
dEnc = m1Raw - m1RawLast; //[pulses]
|
||||
dT = tNow - tLast; //[micro sec]
|
||||
dTurn = dEnc / pulses_per_turn;
|
||||
|
||||
ang_vel = (dTurn * 2 * PI) /(dT*0.000001);
|
||||
lin_vel = (dTurn * wheel_diameter * PI) /(dT*0.000001);
|
||||
//Calculate speed from encoders
|
||||
dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle]
|
||||
dT = tNow - tLast; //[Cycle time in microseconds]
|
||||
dTurn = dEnc / pulses_per_turn; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
|
||||
ang_vel = (dTurn * 2 * PI) / (dT * 0.000001);
|
||||
lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001);
|
||||
|
||||
Serial.print(referenceSpeed);
|
||||
Serial.print ( "," );
|
||||
Serial.println(actualSpeed);
|
||||
// Serial.print ( "," );
|
||||
// Serial.println(speedCMD);
|
||||
// Serial.print ( "," );
|
||||
// Serial.println(lin_vel);
|
||||
// Serial.print ( "," );
|
||||
Serial.print(actualSpeed);
|
||||
Serial.print ( "," );
|
||||
Serial.println(speedCMD);
|
||||
// Serial.print ( "," );
|
||||
// Serial.println(lin_vel);
|
||||
// Serial.print ( "," );
|
||||
|
||||
//update variables for next scan cycle
|
||||
|
||||
//Update variables for next scan cycle
|
||||
m1RawLast = m1Raw;
|
||||
tLast = tNow;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue