100 lines
2.8 KiB
C++
100 lines
2.8 KiB
C++
//Constants
|
|
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
|
|
const float WHEEL_DIAMETER = 0.067708;
|
|
const float PULSES_PER_TURN = 1320.0;
|
|
|
|
//Tuning
|
|
const float K = 3.5;
|
|
const float I = 7.5;
|
|
const float filter_gain = 15;
|
|
|
|
//Help variables
|
|
float M1_Lin_Vel, M2_Lin_Vel;
|
|
int M1_Speed_CMD, M2_Speed_CMD;
|
|
float M1_iError, M2_iError;
|
|
|
|
float ref, act, error;
|
|
|
|
void motors() {
|
|
|
|
//Controllers
|
|
ref = pitch * ((4096.0) / (90.0));
|
|
act = M1_Lin_Vel * 4096.0;
|
|
error = ref - act;
|
|
M1_iError = M1_iError + (error * dT * pow(10, -6) * I);
|
|
M1_Speed_CMD = round((error * K) + M1_iError);
|
|
|
|
// ref = pitch * ((4096.0) / (90.0));
|
|
// act = M2_Lin_Vel * 4096.0;
|
|
// error = ref - act;
|
|
// M2_iError = M2_iError + (error * dT * pow(10, -6) * I);
|
|
// M2_Speed_CMD = round((error * K) + M2_iError);
|
|
|
|
|
|
|
|
|
|
//Calculate speed from encoders
|
|
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
|
|
// M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
|
|
|
|
|
|
//Motor 1
|
|
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION);
|
|
|
|
//Motor 2
|
|
// motorSpeed(2, M2_Speed_CMD, MOTOR_SATURATION);
|
|
motorControl(2, 0, MOTOR_SATURATION);
|
|
|
|
|
|
// Serial plotter
|
|
Serial.print("M1_Speed_REF:");
|
|
Serial.print(ref * (100.0 / 4096.0));
|
|
Serial.print(" ");
|
|
Serial.print("M1_Speed_ACT:");
|
|
Serial.print(act * (100.0 / 4096.0));
|
|
Serial.print(" ");
|
|
Serial.print("M1_Speed_CMD:");
|
|
Serial.println(M1_Speed_CMD * (100.0 / 4096.0));
|
|
|
|
|
|
//Update variables for next scan cycle
|
|
m1RawLast = m1Raw;
|
|
m2RawLast = m2Raw;
|
|
}
|
|
|
|
float encoderReader(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, int dT_, float filt_gain_ ) {
|
|
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
|
|
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
|
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001);
|
|
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_);
|
|
}
|
|
|
|
void motorControl(byte motorID, int speedCMD_, int saturation) {
|
|
//Calculate channel
|
|
byte ch1 = motorID * 2 - 1;
|
|
byte ch2 = motorID * 2;
|
|
|
|
|
|
// Speed command saturation
|
|
if (speedCMD_ > saturation) {
|
|
speedCMD_ = saturation;
|
|
}
|
|
else if (speedCMD_ < -saturation) {
|
|
speedCMD_ = -saturation;
|
|
}
|
|
else {
|
|
speedCMD_ = speedCMD_;
|
|
}
|
|
|
|
|
|
//Motor Control
|
|
if (speedCMD_ > 0) {
|
|
ledcWrite(ch1, 0);
|
|
ledcWrite(ch2, speedCMD_);
|
|
}
|
|
else if (speedCMD_ < 0) {
|
|
ledcWrite(ch1, -1 * speedCMD_);
|
|
ledcWrite(ch2, 0);
|
|
}
|
|
}
|