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

Added remote control

This commit is contained in:
Stedd 2019-12-23 21:00:06 +01:00
parent 050c1156f2
commit 9c363bbe3c
4 changed files with 145 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Main/Main.ino
View File

@ -40,12 +40,6 @@ volatile bool M1_A_state, M1_B_state;
volatile bool M2_A_state, M2_B_state;
//Matrices
mtx_type motor_ang_vel [2][1];
mtx_type vel_Matrix [2][1];
mtx_type inv_Kin [2][2];
void setup() {
//Initialize serial
Serial.begin(57600);
@ -71,6 +65,7 @@ void setup() {
m2Raw = 0;
m2RawLast = 100;
// Initialize PWM channels
ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2);
@ -82,9 +77,14 @@ void setup() {
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
//Initialize differential drive inverse kinematics
initMotors();
// Initialize Remote control
initRemote();
}
void loop() {
@ -98,6 +98,10 @@ void loop() {
readIMU();
//Get remote control data
readRemote();
//Control motors
motors();
@ -105,8 +109,9 @@ void loop() {
//Save time for next cycle
tLast = tNow;
//Plot
// plot();
// Plot
plot();
//Delay

69
Main/interruptRemote.ino Normal file
View File

@ -0,0 +1,69 @@
//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;
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 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);
//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;
}

23
Main/motorControl.ino
View File

@ -14,7 +14,7 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning
const float K_SC = 20.0;
const float K_TC = 50.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;
@ -23,12 +23,19 @@ const float filter_gain = 16.0;
//Help variables
int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref;
float ref_SC, act_SC, error_SC, SC_cont_out;
float ref_TC, act_TC, error_TC, TC_cont_out;
float ref_OL, act_OL, error_OL, OL_cont_out;
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL;
//Matrices
mtx_type motor_ang_vel [2][1];
mtx_type vel_Matrix [2][1];
mtx_type inv_Kin [2][2];
void initMotors() {
inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
@ -48,8 +55,13 @@ void motors() {
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
// Remote control commands
rem_turn_speed_ref = remoteCMD[0];
rem_speed_ref = remoteCMD[1];
// Speed Controller
ref_SC = SPEED_REF;
// 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;
@ -70,7 +82,8 @@ void motors() {
//Turn controller
ref_TC = TURN_SPEED_REF;
// 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;
@ -80,6 +93,10 @@ void motors() {
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;
// M2_Speed_CMD = 0;
//Motor control
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);

32
Main/plot.ino
View File

@ -1,11 +1,11 @@
void plot(){
// Time
// Serial.print("dT:");
// Serial.print(dT);
// Serial.println(dT);
// Serial.print(" ");
// Serial.print("dT_s:");
// Serial.println(dT_s);
// Serial.print(" ");
// IMU
// Serial.print ( "Pitch:" );
@ -18,10 +18,25 @@ void plot(){
// Serial.println ( gz );
// Serial.print ( "," );
// Serial.println ( gt );
// Serial.print ( "," );
// Serial.print ( " " );
// Serial.println ( acc_pitch);
// Remote control
// Serial.print("ch1:");
// Serial.print(remoteCMD[0]);
// Serial.print(" ");
// Serial.print("ch2:");
// Serial.println(remoteCMD[1]);
// Encoders
// Serial.print("m1Raw:");
// Serial.print(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
// Motors
// Serial plotter
// Serial.print("Balance_Point:");
@ -34,21 +49,14 @@ void plot(){
// Serial.println(Speed_CMD * (100.0 / 4096.0));
// Serial.print("M1_Ang_Vel:");
// Serial.print(M1_Ang_Vel);
// Serial.print(motor_ang_vel[0][0]);
// Serial.print(" ");
// Serial.print("M2_Ang_Vel:");
// Serial.print(M2_Ang_Vel);
// Serial.print(motor_ang_vel[0][1]);
// Serial.print(" ");
// Serial.print("botLinVel:");
// Serial.print(vel_Matrix[0][0]);
// Serial.print(" ");
// Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]);
// Encoders
// Serial.print("m1Raw:");
// Serial.print(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
}