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

Cleanup and tuning

This commit is contained in:
Stedd 2019-12-24 01:21:12 +01:00
parent 9c363bbe3c
commit 7dffe5208d
4 changed files with 71 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Main/IMU.ino
View File

@ -11,7 +11,6 @@ int gx, gy, gz;
float gt; float gt;
float acc_pitch; float acc_pitch;
float pitch_rate; float pitch_rate;
float dAngle, estAngle;
float pitch = 0; float pitch = 0;
float pitch_prev = 0; float pitch_prev = 0;
@ -48,8 +47,7 @@ void readIMU() {
//Complementary filter //Complementary filter
dAngle = pitch_rate * dT_s; pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch = acc_pitch * (1 - alpha) + ((dAngle + pitch_prev) * alpha);
pitch_prev = pitch; pitch_prev = pitch;

33
Main/Main.ino
View File

@ -30,7 +30,7 @@ float dT_s = 0.0;
//Motor variables //Motor variables
const int PWM_CYCLE = 12000; const int PWM_CYCLE = 12000;
const byte PWM_RESOLUTION = 12; const byte PWM_RES = 12;
//Encoders variables //Encoders variables
@ -58,30 +58,31 @@ void setup() {
//Initialize encoder interrupts //Initialize encoder interrupts
initInterrupt(); initInterrupt();
//Initialize encoders //Initialize encoders
m1Raw = 0; m1Raw = 0;
m1RawLast = 100; m1RawLast = 100;
m2Raw = 0; m2Raw = 0;
m2RawLast = 100; m2RawLast = 100;
// Initialize PWM channels // Initialize PWM channels
// byte pwmPins[4] = {M1_A, M1_B, M2_A, M2_B};
// for(int i = 1; i >= 4; i++){
// ledcAttachPin(pwmPins[i-1], i);
// ledcSetup(i, PWM_CYCLE, PWM_RES);
// }
ledcAttachPin(M1_A, 1); ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2); ledcAttachPin(M1_B, 2);
ledcAttachPin(M2_A, 3); ledcAttachPin(M2_A, 3);
ledcAttachPin(M2_B, 4); ledcAttachPin(M2_B, 4);
ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(1, PWM_CYCLE, PWM_RES);
ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(2, PWM_CYCLE, PWM_RES);
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(3, PWM_CYCLE, PWM_RES);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(4, PWM_CYCLE, PWM_RES);
//Initialize differential drive inverse kinematics //Initialize differential drive inverse kinematics
initMotors(); initMotors();
// Initialize Remote control // Initialize Remote control
initRemote(); initRemote();
@ -98,22 +99,18 @@ void loop() {
readIMU(); readIMU();
//Get remote control data
readRemote();
//Control motors //Control motors
motors(); motors();
//Save time for next cycle
tLast = tNow;
// Plot // Plot
plot(); plot();
//Save time for next cycle
tLast = tNow;
//Delay //Delay
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz delay(5);
} }

35
Main/interruptRemote.ino
View File

@ -1,12 +1,6 @@
//Variables //Variables
const byte NO_CHANNELS = 2; const byte NO_CHANNELS = 2;
const byte CHANNEL_PINS[] = {12, 14}; const byte CHANNEL_PINS[] = {12, 14};
const int CH_MIN = 980;
const int CH_MAX = 1997;
const int INIT_VALUE = CH_MAX/2;
float remoteRaw[NO_CHANNELS];
float remoteCMD[NO_CHANNELS];
volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1; volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1;
volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2; volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2; volatile unsigned int pwm_time_ch1, pwm_time_ch2;
@ -21,6 +15,7 @@ volatile unsigned int pwm_time_ch1, pwm_time_ch2;
interruptTimeLast_ch1 = interruptTime_ch1; interruptTimeLast_ch1 = interruptTime_ch1;
} }
void ch2_interrupt() { void ch2_interrupt() {
interruptTime_ch2 = micros(); interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){ if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
@ -31,7 +26,6 @@ volatile unsigned int pwm_time_ch1, pwm_time_ch2;
void initRemote(){ void initRemote(){
//Ch1 //Ch1
pinMode(CHANNEL_PINS[0], INPUT_PULLUP); pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE); attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
@ -39,31 +33,4 @@ void initRemote(){
//Ch2 //Ch2
pinMode(CHANNEL_PINS[1], INPUT); pinMode(CHANNEL_PINS[1], INPUT);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE); attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE);
}
void readRemote(){
remoteCMD[0] = floatMap(pwm_time_ch1, 992.0, 2007.0, -2.5, 2.5); //turn rate
remoteCMD[1] = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25); //speed
// Remote control
// Serial.print("ch1:");
// Serial.print(pwm_time_ch1);
// Serial.print(" ");
// Serial.print("ch2:");
// Serial.print(pwm_time_ch2);
// Serial.print(" ");
// Serial.print("ch1_mapped:");
// Serial.print(remoteCMD[0]);
// Serial.print(" ");
// Serial.print("ch2_mapped:");
// Serial.println(remoteCMD[1]);
}
float floatMap(int in, float inMin, float inMax, float outMin, float outMax){
return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
} }

46
Main/motorControl.ino
View File

@ -1,5 +1,5 @@
//Constants //Constants
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION)); const int MOTOR_SATURATION = round(pow(2, PWM_RES));
const float BASE_WIDTH = 0.1837; const float BASE_WIDTH = 0.1837;
const float WHEEL_DIAMETER = 0.0677; const float WHEEL_DIAMETER = 0.0677;
const float PULSES_PER_TURN = 1320.0; const float PULSES_PER_TURN = 1320.0;
@ -13,12 +13,12 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float K_SC = 20.0; const float K_SC = 18.0; //Speed controller gain
const float K_TC = 100.0; const float K_TC = 130.0; //Turn controller gain
const float K_OL = 13.0; const float K_OL = 14.0; //Outer loop balance controller gain
const float K_IL = 85.0; const float K_IL = 85.0; //Inner loop balance controller gain
const float I_IL = 5.25; const float I_IL = 5.25; //Inner loop balance controller Igain
const float filter_gain = 16.0; const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables //Help variables
@ -37,6 +37,7 @@ mtx_type inv_Kin [2][2];
void initMotors() { void initMotors() {
// Inverse Kinematic matrix of differential drive robot
inv_Kin[0][0] = WHEEL_DIAMETER / 4; inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH; inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4; inv_Kin[0][1] = WHEEL_DIAMETER / 4;
@ -56,23 +57,17 @@ void motors() {
// Remote control commands // Remote control commands
rem_turn_speed_ref = remoteCMD[0]; rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5);
rem_speed_ref = remoteCMD[1]; rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25);
// Speed Controller // Speed Controller
// ref_SC = SPEED_REF; SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
ref_SC = rem_speed_ref;
act_SC = vel_Matrix[0][0];
error_SC = ref_SC - act_SC;
SC_cont_out = error_SC * K_SC;
// Balance controller // Balance controller
// Outer loop // Outer loop
ref_OL = BALANCE_POINT - SC_cont_out; OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
act_OL = pitch;
error_OL = ref_OL - act_OL;
OL_cont_out = error_OL * K_OL;
// Inner loop // Inner loop
ref_IL = OL_cont_out; ref_IL = OL_cont_out;
act_IL = pitch_rate; act_IL = pitch_rate;
@ -82,11 +77,7 @@ void motors() {
//Turn controller //Turn controller
// ref_TC = TURN_SPEED_REF; TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
ref_TC = rem_turn_speed_ref;
act_TC = vel_Matrix[0][1];
error_TC = ref_TC - act_TC;
TC_cont_out = error_TC * K_TC;
//Sum speed command for motors //Sum speed command for motors
@ -110,6 +101,15 @@ void motors() {
} }
float PController(float ref_, float act_, float k_){
return (ref_-act_)*k_;
}
float floatMap(int in, float inMin, float inMax, float outMin, float outMax){
return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
}
float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) { float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float 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]