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 acc_pitch;
float pitch_rate;
float dAngle, estAngle;
float pitch = 0;
float pitch_prev = 0;
@ -48,8 +47,7 @@ void readIMU() {
//Complementary filter
dAngle = pitch_rate * dT_s;
pitch = acc_pitch * (1 - alpha) + ((dAngle + pitch_prev) * alpha);
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = pitch;

35
Main/Main.ino
View File

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

71
Main/interruptRemote.ino
View File

@ -1,37 +1,31 @@
//Variables
const byte NO_CHANNELS = 2;
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_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2;
const byte NO_CHANNELS = 2;
const byte CHANNEL_PINS[] = {12, 14};
volatile unsigned int interruptTime_ch1, interruptTimeLast_ch1;
volatile unsigned int interruptTime_ch2, interruptTimeLast_ch2;
volatile unsigned int pwm_time_ch1, pwm_time_ch2;
void ch1_interrupt() {
interruptTime_ch1 = micros();
if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
}
interruptTimeLast_ch1 = interruptTime_ch1;
void ch1_interrupt() {
interruptTime_ch1 = micros();
if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
}
void ch2_interrupt() {
interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
}
interruptTimeLast_ch2 = interruptTime_ch2;
interruptTimeLast_ch1 = interruptTime_ch1;
}
void ch2_interrupt() {
interruptTime_ch2 = micros();
if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
}
interruptTimeLast_ch2 = interruptTime_ch2;
}
void initRemote(){
//Ch1
pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
@ -39,31 +33,4 @@ void initRemote(){
//Ch2
pinMode(CHANNEL_PINS[1], INPUT);
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;
}

68
Main/motorControl.ino
View File

@ -1,5 +1,5 @@
//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 WHEEL_DIAMETER = 0.0677;
const float PULSES_PER_TURN = 1320.0;
@ -13,12 +13,12 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning
const float K_SC = 20.0;
const float K_TC = 100.0;
const float K_OL = 13.0;
const float K_IL = 85.0;
const float I_IL = 5.25;
const float filter_gain = 16.0;
const float K_SC = 18.0; //Speed controller gain
const float K_TC = 130.0; //Turn controller gain
const float K_OL = 14.0; //Outer loop balance controller gain
const float K_IL = 85.0; //Inner loop balance controller gain
const float I_IL = 5.25; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables
@ -37,10 +37,11 @@ mtx_type inv_Kin [2][2];
void initMotors() {
inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4;
inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH;
// Inverse Kinematic matrix of differential drive robot
inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4;
inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH;
}
void motors() {
@ -56,42 +57,32 @@ void motors() {
// Remote control commands
rem_turn_speed_ref = remoteCMD[0];
rem_speed_ref = remoteCMD[1];
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5);
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25);
// Speed Controller
// ref_SC = SPEED_REF;
ref_SC = rem_speed_ref;
act_SC = vel_Matrix[0][0];
error_SC = ref_SC - act_SC;
SC_cont_out = error_SC * K_SC;
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// 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;
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, 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_s * I_IL);
IL_cont_out = round((error_IL * K_IL) + iError_IL);
ref_IL = OL_cont_out;
act_IL = pitch_rate;
error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (error_IL * dT_s * I_IL);
IL_cont_out = round((error_IL * K_IL) + iError_IL);
//Turn controller
// ref_TC = TURN_SPEED_REF;
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;
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
//Sum speed command for motors
M1_Speed_CMD = IL_cont_out - TC_cont_out;
M2_Speed_CMD = IL_cont_out + TC_cont_out;
M1_Speed_CMD = IL_cont_out - TC_cont_out;
M2_Speed_CMD = IL_cont_out + TC_cont_out;
//Sum speed command for motors
// M1_Speed_CMD = 0;
@ -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 dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]