Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Revert "asdf" 

This reverts commit f1719eca0a.
This commit is contained in:
Stedd 2019-12-21 00:06:05 +01:00
parent f1719eca0a
commit e317f3616b
1 changed files with 36 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
Main/motorControl.ino
View File

@ -2,76 +2,59 @@
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION)); const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
const float WHEEL_DIAMETER = 0.067708; const float WHEEL_DIAMETER = 0.067708;
const float PULSES_PER_TURN = 1320.0; const float PULSES_PER_TURN = 1320.0;
const float BALANCE_POINT = -0.05;
const float SPEED_REFERENCE = 0.0;
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;
//Tuning //Tuning
const float K_SC = 15.0; const float K = 3.5;
const float K_OL = 13.0; const float I = 7.5;
const float K_IL = 80.0; const float filter_gain = 15;
const float I_IL = 5.5;
const float filter_gain = 15.0;
//Help variables //Help variables
float M1_Lin_Vel, M2_Lin_Vel; float M1_Lin_Vel, M2_Lin_Vel;
int Speed_CMD, M1_Speed_CMD, M2_Speed_CMD; int M1_Speed_CMD, M2_Speed_CMD;
float M1_iError, M2_iError;
float ref_SC, act_SC, error_SC, SC_cont_out; float ref, act, error;
float ref_OL, act_OL, error_OL, OL_cont_out;
float ref_IL, act_IL, error_IL, iError_IL;
void motors() { 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);
// Speed Controller // ref = pitch * ((4096.0) / (90.0));
ref_SC = SPEED_REFERENCE; // act = M2_Lin_Vel * 4096.0;
act_SC = (M1_Lin_Vel + M2_Lin_Vel) / 2; // error = ref - act;
error_SC = ref_SC - act_SC; // M2_iError = M2_iError + (error * dT * pow(10, -6) * I);
SC_cont_out = (error_SC * K_SC); // M2_Speed_CMD = round((error * K) + M2_iError);
// Balance controller
// Outer loop
ref_OL = BALANCE_POINT - SC_cont_out;
act_OL = pitch;
error_OL = ref_OL - act_OL;
OL_cont_out = (error_OL * K_OL);
// Inner loop
ref_IL = OL_cont_out;
act_IL = pitch_rate;
error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (error_IL * dT * pow(10, -6) * I_IL);
Speed_CMD = round((error_IL * K_IL) + iError_IL);
M1_Speed_CMD = Speed_CMD;
M2_Speed_CMD = Speed_CMD;
// M1_Speed_CMD = 0;
// M2_Speed_CMD = 0;
//Calculate speed from encoders //Calculate speed from encoders
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain); 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); // M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
//Motor control //Motor 1
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); motorControl(1, M1_Speed_CMD, MOTOR_SATURATION);
motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
//Motor 2
// motorSpeed(2, M2_Speed_CMD, MOTOR_SATURATION);
motorControl(2, 0, MOTOR_SATURATION);
// Serial plotter // Serial plotter
Serial.print("Balance_Point:"); Serial.print("M1_Speed_REF:");
Serial.print(ref_OL); Serial.print(ref * (100.0 / 4096.0));
Serial.print(" "); Serial.print(" ");
Serial.print("Pitch_Angle:"); Serial.print("M1_Speed_ACT:");
Serial.print(act_OL); Serial.print(act * (100.0 / 4096.0));
Serial.print(" "); Serial.print(" ");
Serial.print("Speed_CMD:"); Serial.print("M1_Speed_CMD:");
Serial.println(Speed_CMD * (100.0 / 4096.0)); Serial.println(M1_Speed_CMD * (100.0 / 4096.0));
//Update variables for next scan cycle //Update variables for next scan cycle
@ -80,13 +63,13 @@ void motors() {
} }
float encoderReader(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, int dT_, float filt_gain_ ) { 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 dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation] float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001); float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001);
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_); return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_);
} }
void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { void motorControl(byte motorID, int speedCMD_, int saturation) {
//Calculate channel //Calculate channel
byte ch1 = motorID * 2 - 1; byte ch1 = motorID * 2 - 1;
byte ch2 = motorID * 2; byte ch2 = motorID * 2;
@ -99,26 +82,12 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
else if (speedCMD_ < -saturation) { else if (speedCMD_ < -saturation) {
speedCMD_ = -saturation; speedCMD_ = -saturation;
} }
//Deadband
else if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_;
}
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
speedCMD_ = -dbNeg_;
}
//Zero speed if input = 0
else if (speedCMD_ == 0) {
speedCMD_ = 0;
}
else { else {
speedCMD_ = speedCMD_; speedCMD_ = speedCMD_;
} }
//Apply speed command to PWM output //Motor Control
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, speedCMD_); ledcWrite(ch2, speedCMD_);