176 lines
3.5 KiB
C++
176 lines
3.5 KiB
C++
//Import
|
|
#include <GY_85.h>
|
|
#include <Wire.h>
|
|
#include <MatrixMath.h>
|
|
#include <Ps3Controller.h>
|
|
#include "arduino_secrets.h"
|
|
|
|
//Declare library objects
|
|
GY_85 IMU;
|
|
|
|
|
|
//GPIO PIN MAPPING
|
|
const byte M1_ENC_A = 32;
|
|
const byte M1_ENC_B = 33;
|
|
const byte M2_ENC_A = 34;
|
|
const byte M2_ENC_B = 35;
|
|
const byte M1_A = 16;
|
|
const byte M1_B = 17;
|
|
const byte M2_A = 18;
|
|
const byte M2_B = 19;
|
|
const int IMU_I2C_SDA = 26;
|
|
const int IMU_I2C_SCL = 27;
|
|
|
|
|
|
//Time variables
|
|
unsigned long tNow = micros();
|
|
unsigned long tLast = micros() + 13000;
|
|
int dT = 0;
|
|
float dT_s = 0.0;
|
|
|
|
|
|
//Encoder variables
|
|
long int m1Raw, m1RawLast;
|
|
long int m2Raw, m2RawLast;
|
|
volatile bool M1_A_state, M1_B_state;
|
|
volatile bool M2_A_state, M2_B_state;
|
|
|
|
|
|
//PS3 Controller variables
|
|
const char* _ps3Address = "18:5e:0f:92:00:6c";
|
|
|
|
|
|
//MotorControl Constants
|
|
const int PWM_CYCLE = 12000;
|
|
const byte PWM_RES = 12;
|
|
const int MOTOR_SATURATION = round(pow(2, PWM_RES));
|
|
const float BASE_WIDTH = 0.1837;
|
|
const float WHEEL_DIAMETER = 0.0677;
|
|
const float PULSES_PER_TURN = 1320.0;
|
|
const float BALANCE_POINT = 0.05;
|
|
const float SPEED_REF = 0.00;
|
|
const float TURN_SPEED_REF = 0.00;
|
|
const float DEADBAND_M1_POS = 90.0;
|
|
const float DEADBAND_M1_NEG = 90.0;
|
|
const float DEADBAND_M2_POS = 90.0;
|
|
const float DEADBAND_M2_NEG = 90.0;
|
|
|
|
|
|
//MotorControl Tuning
|
|
const float gainScale = 1;
|
|
const float K_SC = 18.5 * gainScale; //Speed controller gain
|
|
const float K_TC = 90.0 * gainScale; //Turn controller gain
|
|
const float K_OL = 13.0 * gainScale; //Outer loop balance controller gain
|
|
const float K_IL = 72.0 * gainScale; //Inner loop balance controller gain
|
|
const float I_IL = 80.0 * gainScale; //Inner loop balance controller Igain
|
|
const float filter_gain = 16.0; //Motor speed LPF gain
|
|
|
|
|
|
//MotorControl Help variables
|
|
int M1_Speed_CMD, M2_Speed_CMD;
|
|
float rem_speed_ref, rem_turn_speed_ref;
|
|
float SC_cont_out;
|
|
float TC_cont_out;
|
|
float OL_cont_out;
|
|
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
|
|
float speedCmd1, speedCmd2;
|
|
bool balancingOn = false;
|
|
|
|
|
|
//MotorControl Matrices
|
|
mtx_type motor_ang_vel[2][1];
|
|
mtx_type vel_Matrix[2][1];
|
|
mtx_type inv_Kin[2][2];
|
|
|
|
|
|
//IMU CONSTANTS
|
|
const float alpha = 0.95;
|
|
const int acc_overflow_value = 65535;
|
|
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
|
|
|
|
|
|
//IMU VARIABLES
|
|
int ax, ay, az;
|
|
int cx, cy, cz;
|
|
int gx, gy, gz;
|
|
float gt;
|
|
float acc_pitch;
|
|
float pitch_rate;
|
|
float pitch = 0;
|
|
float pitch_prev = 0;
|
|
|
|
|
|
//UDP variables
|
|
uint8_t data[30 * 4];
|
|
|
|
|
|
void setup() {
|
|
|
|
//Initialize serial
|
|
Serial.begin(9600);
|
|
delay(10);
|
|
|
|
//Initialice I2C
|
|
Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
|
|
delay(10);
|
|
|
|
//Initialize IMU
|
|
IMU.init();
|
|
delay(10);
|
|
|
|
//Initialize encoder interrupts
|
|
initEncoderInterrupt();
|
|
|
|
//Initialize encoders
|
|
m1Raw = 0;
|
|
m1RawLast = 100;
|
|
m2Raw = 0;
|
|
m2RawLast = 100;
|
|
|
|
// Initialize PWM channels
|
|
ledcAttachPin(M1_A, 1);
|
|
ledcAttachPin(M1_B, 2);
|
|
ledcAttachPin(M2_A, 3);
|
|
ledcAttachPin(M2_B, 4);
|
|
|
|
ledcSetup(1, PWM_CYCLE, PWM_RES);
|
|
ledcSetup(2, PWM_CYCLE, PWM_RES);
|
|
ledcSetup(3, PWM_CYCLE, PWM_RES);
|
|
ledcSetup(4, PWM_CYCLE, PWM_RES);
|
|
|
|
//Initialize differential drive inverse kinematics
|
|
initMotors();
|
|
|
|
//Initialize PS3 controller connection
|
|
Ps3.begin(_ps3Address);
|
|
|
|
//Init UDP
|
|
UdpInit();
|
|
}
|
|
|
|
void loop() {
|
|
|
|
//Update time variables
|
|
tNow = micros();
|
|
dT = tNow - tLast; //[Cycle time in microseconds]
|
|
dT_s = dT * pow(10, -6); //[Cycle time in seconds]
|
|
|
|
//Get sensor data
|
|
readIMU();
|
|
|
|
//Control motors
|
|
motors();
|
|
|
|
// Plot
|
|
SerialPlot();
|
|
|
|
//Udp
|
|
UdpLoop();
|
|
|
|
//Save time for next cycle
|
|
tLast = tNow;
|
|
|
|
//Delay
|
|
delay(5);
|
|
}
|