From b5a53fa8c9a863bc5f7eef0eed28b50530b2ad37 Mon Sep 17 00:00:00 2001 From: Stedd Date: Wed, 18 Oct 2023 22:16:53 +0200 Subject: [PATCH] Cleanup --- Main/IMU.ino | 50 +++++++------ Main/Main.ino | 57 ++++++++------- Main/interruptEncoders.ino | 35 ++++----- Main/motorControl.ino | 141 ++++++++++++++++++------------------- Main/plot.ino | 130 +++++++++++++++++----------------- 5 files changed, 199 insertions(+), 214 deletions(-) diff --git a/Main/IMU.ino b/Main/IMU.ino index b42cf88..83ff38b 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -1,41 +1,41 @@ //CONSTANTS -const float alpha = 0.95; -const int acc_overflow_value = 65535; -const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? +const float alpha = 0.95; +const int acc_overflow_value = 65535; +const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? //IMU VARIABLES -int ax, ay, az; -int cx, cy, cz; -int gx, gy, gz; -float gt; -float acc_pitch; -float pitch_rate; -float pitch = 0; -float pitch_prev = 0; +int ax, ay, az; +int cx, cy, cz; +int gx, gy, gz; +float gt; +float acc_pitch; +float pitch_rate; +float pitch = 0; +float pitch_prev = 0; void readIMU() { //Acceletometer - ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value); - ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value); - az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value); + ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value); //Magnetometer - cx = IMU.compass_x( IMU.readFromCompass() ); - cy = IMU.compass_y( IMU.readFromCompass() ); - cz = IMU.compass_z( IMU.readFromCompass() ); + cx = IMU.compass_x(IMU.readFromCompass()); + cy = IMU.compass_y(IMU.readFromCompass()); + cz = IMU.compass_z(IMU.readFromCompass()); // Gyrocope - gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate - gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate - gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate + gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate //Temperature sensor - gt = IMU.temp ( IMU.readGyro() ); + gt = IMU.temp(IMU.readGyro()); // Pitch angle from accelerometer @@ -47,10 +47,8 @@ void readIMU() { //Complementary filter - pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); - pitch_prev = pitch; - - + pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); + pitch_prev = pitch; } @@ -61,7 +59,7 @@ int convertInt(int raw, int overflow_value_) { return (raw - overflow_value_); } - else { + else { return raw; } } diff --git a/Main/Main.ino b/Main/Main.ino index f7c10e5..7a2c05c 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -6,39 +6,39 @@ #include //Declare library objects -GY_85 IMU; +GY_85 IMU; //GPIO PIN MAPPING -const byte M1_ENC_A = 32; -const byte M1_ENC_B = 33; -const byte M2_ENC_A = 34; -const byte M2_ENC_B = 35; -const byte M1_A = 16; -const byte M1_B = 17; -const byte M2_A = 18; -const byte M2_B = 19; -const byte IMU_I2C_SCL = 26; -const byte IMU_I2C_SDA = 27; +const byte M1_ENC_A = 32; +const byte M1_ENC_B = 33; +const byte M2_ENC_A = 34; +const byte M2_ENC_B = 35; +const byte M1_A = 16; +const byte M1_B = 17; +const byte M2_A = 18; +const byte M2_B = 19; +const byte IMU_I2C_SCL = 26; +const byte IMU_I2C_SDA = 27; //Time variables -unsigned long tNow = micros(); -unsigned long tLast = micros() + 13000; -int dT = 0; -float dT_s = 0.0; +unsigned long tNow = micros(); +unsigned long tLast = micros() + 13000; +int dT = 0; +float dT_s = 0.0; //Motor variables -const int PWM_CYCLE = 12000; -const byte PWM_RES = 12; +const int PWM_CYCLE = 12000; +const byte PWM_RES = 12; //Encoders variables -long int m1Raw, m1RawLast; -long int m2Raw, m2RawLast; -volatile bool M1_A_state, M1_B_state; -volatile bool M2_A_state, M2_B_state; +long int m1Raw, m1RawLast; +long int m2Raw, m2RawLast; +volatile bool M1_A_state, M1_B_state; +volatile bool M2_A_state, M2_B_state; //PS3 Controller variables const char* _ps3Address = "18:5e:0f:92:00:6c"; @@ -54,12 +54,10 @@ void setup() { //Initialize IMU IMU.init(); - //Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values... - IMU.GyroCalibrate(); delay(10); //Initialize encoder interrupts - initInterrupt(); + initEncoderInterrupt(); //Initialize encoders m1Raw = 0; @@ -83,14 +81,13 @@ void setup() { //Initialize PS3 controller connection Ps3.begin(_ps3Address); - } void loop() { //Update time variables - tNow = micros(); - dT = tNow - tLast; //[Cycle time in microseconds] - dT_s = dT * pow(10,-6); //[Cycle time in seconds] + tNow = micros(); + dT = tNow - tLast; //[Cycle time in microseconds] + dT_s = dT * pow(10, -6); //[Cycle time in seconds] //Get sensor data @@ -102,7 +99,7 @@ void loop() { // Plot - plot(); + //plot(); //Save time for next cycle @@ -111,4 +108,6 @@ void loop() { //Delay delay(5); + + //Test } diff --git a/Main/interruptEncoders.ino b/Main/interruptEncoders.ino index 29ca3a7..12aa1a7 100644 --- a/Main/interruptEncoders.ino +++ b/Main/interruptEncoders.ino @@ -7,8 +7,7 @@ void IRAM_ATTR m1_A_changed() { if (M1_A_state == HIGH) { if (M1_B_state == HIGH) { m1Raw = m1Raw - 1; - } - else if (M1_B_state == LOW) { + } else if (M1_B_state == LOW) { m1Raw = m1Raw + 1; } } @@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() { else if (M1_A_state == LOW) { if (M1_B_state == HIGH) { m1Raw = m1Raw + 1; - } - else if (M1_B_state == LOW) { + } else if (M1_B_state == LOW) { m1Raw = m1Raw - 1; } } @@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() { if (M1_B_state == HIGH) { if (M1_A_state == HIGH) { m1Raw = m1Raw + 1; - } - else if (M1_A_state == LOW) { + } else if (M1_A_state == LOW) { m1Raw = m1Raw - 1; } } @@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() { else if (M1_B_state == LOW) { if (M1_A_state == HIGH) { m1Raw = m1Raw - 1; - } - else if (M1_A_state == LOW) { + } else if (M1_A_state == LOW) { m1Raw = m1Raw + 1; } } @@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() { if (M2_A_state == HIGH) { if (M2_B_state == HIGH) { m2Raw = m2Raw + 1; - } - else if (M2_B_state == LOW) { + } else if (M2_B_state == LOW) { m2Raw = m2Raw - 1; } } @@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() { else if (M2_A_state == LOW) { if (M2_B_state == HIGH) { m2Raw = m2Raw - 1; - } - else if (M2_B_state == LOW) { + } else if (M2_B_state == LOW) { m2Raw = m2Raw + 1; } } @@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() { if (M2_B_state == HIGH) { if (M2_A_state == HIGH) { m2Raw = m2Raw - 1; - } - else if (M2_A_state == LOW) { + } else if (M2_A_state == LOW) { m2Raw = m2Raw + 1; } } @@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() { else if (M2_B_state == LOW) { if (M2_A_state == HIGH) { m2Raw = m2Raw + 1; - } - else if (M2_A_state == LOW) { + } else if (M2_A_state == LOW) { m2Raw = m2Raw - 1; } } } -void initInterrupt(){ +void initEncoderInterrupt() { pinMode(M1_ENC_A, INPUT_PULLUP); - pinMode(M1_ENC_B, INPUT_PULLUP); - pinMode(M2_ENC_A, INPUT_PULLUP); - pinMode(M2_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); + + pinMode(M1_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE); + + pinMode(M2_ENC_A, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE); + + pinMode(M2_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); } diff --git a/Main/motorControl.ino b/Main/motorControl.ino index c958aa8..60694ab 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -1,48 +1,48 @@ //Constants -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; -const float BALANCE_POINT = 0.05; -const float SPEED_REF = 0.00; -const float TURN_SPEED_REF = 0.00; -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; +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; +const float BALANCE_POINT = 0.05; +const float SPEED_REF = 0.00; +const float TURN_SPEED_REF = 0.00; +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 -const float K_SC = 18.5; //Speed controller gain -const float K_TC = 90.0; //Turn controller gain -const float K_OL = 13.0; //Outer loop balance controller gain -const float K_IL = 72.0; //Inner loop balance controller gain -const float I_IL = 80.0; //Inner loop balance controller Igain -const float filter_gain = 16.0; //Motor speed LPF gain +const float K_SC = 18.5; //Speed controller gain +const float K_TC = 90.0; //Turn controller gain +const float K_OL = 13.0; //Outer loop balance controller gain +const float K_IL = 72.0; //Inner loop balance controller gain +const float I_IL = 80.0; //Inner loop balance controller Igain +const float filter_gain = 16.0; //Motor speed LPF gain //Help variables -int M1_Speed_CMD, M2_Speed_CMD; -float rem_speed_ref, rem_turn_speed_ref; -float SC_cont_out; -float TC_cont_out; -float OL_cont_out; -float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; +int M1_Speed_CMD, M2_Speed_CMD; +float rem_speed_ref, rem_turn_speed_ref; +float SC_cont_out; +float TC_cont_out; +float OL_cont_out; +float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; //Matrices -mtx_type motor_ang_vel [2][1]; -mtx_type vel_Matrix [2][1]; -mtx_type inv_Kin [2][2]; +mtx_type motor_ang_vel[2][1]; +mtx_type vel_Matrix[2][1]; +mtx_type inv_Kin[2][2]; void initMotors() { // 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; + 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() { @@ -57,93 +57,89 @@ void motors() { Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); - rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); // Speed Controller - SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); + SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); // Balance controller // Outer loop - OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, 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 + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_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 + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); + IL_cont_out = round((error_IL * K_IL) + iError_IL); //Turn controller - TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], 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; - // M2_Speed_CMD = 0; + M1_Speed_CMD = 0; + M2_Speed_CMD = 0; //Motor control IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - IL_anti_windup = IL_anti_windup/2; + IL_anti_windup = IL_anti_windup / 2; //Update variables for next scan cycle m1RawLast = m1Raw; m2RawLast = m2Raw; - - } -float PController(float ref_, float act_, float k_){ - return (ref_-act_)*k_; +float PController(float ref_, float act_, float k_) { + return (ref_ - act_) * k_; } -float floatMap(int in, float inMin, float inMax, float outMin, float outMax){ +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] - float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_); - return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 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 dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation] + float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_); + return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_); } -float encoderReaderAngVel(int encRaw, int encRawLast, float ang_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] - float ang_vel_ = (dTurn_ * 2 * PI) / (dT_); - return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); +float encoderReaderAngVel(int encRaw, int encRawLast, float ang_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] + float ang_vel_ = (dTurn_ * 2 * PI) / (dT_); + return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); } float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { //Returns anti windup difference //Calculate channel - byte ch2 = motorID * 2; - byte ch1 = ch2 - 1; - float windup = 0; + byte ch2 = motorID * 2; + byte ch1 = ch2 - 1; + float windup = 0; //Deadband if (speedCMD_ > 0 && speedCMD_ < dbPos_) { speedCMD_ = dbPos_; - } - else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { + } else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { speedCMD_ = -dbNeg_; } // Speed command saturation else if (speedCMD_ > saturation) { - windup = saturation-speedCMD_; + windup = saturation - speedCMD_; speedCMD_ = saturation; - } - else if (speedCMD_ < -saturation) { - windup = saturation-speedCMD_; + } else if (speedCMD_ < -saturation) { + windup = saturation - speedCMD_; speedCMD_ = -saturation; } @@ -155,16 +151,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl if (speedCMD_ > 0) { ledcWrite(ch1, 0); ledcWrite(ch2, speedCMD_); - } - else if (speedCMD_ < 0) { + } else if (speedCMD_ < 0) { ledcWrite(ch1, -1 * speedCMD_); ledcWrite(ch2, 0); - } - else if (speedCMD_ == 0) { + } else if (speedCMD_ == 0) { ledcWrite(ch1, 0); ledcWrite(ch2, 0); } return windup; - } diff --git a/Main/plot.ino b/Main/plot.ino index 96b5ce0..5217571 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -1,77 +1,77 @@ -void plot(){ -// Time - // Serial.print("dT:"); - // Serial.println(dT); - // Serial.print(" "); - // Serial.print("dT_s:"); - // Serial.println(dT_s); - // Serial.print(" "); +void plot() { + // Time + // Serial.print("dT:"); + // Serial.println(dT); + // Serial.print(" "); + // Serial.print("dT_s:"); + // Serial.println(dT_s); + // Serial.print(" "); -// IMU - // Serial.print ( "Pitch:" ); - // Serial.print ( pitch ); - // Serial.print (" "); - // Serial.print ( "Accelerometer_Pitch:" ); - // Serial.print ( acc_pitch ); - // Serial.print (" "); - // Serial.print ( "," ); - // Serial.println ( gz ); - // Serial.print ( "," ); - // Serial.println ( gt ); - // Serial.print ( " " ); - // Serial.println ( acc_pitch); + // IMU + // Serial.print ( "Pitch:" ); + // Serial.print ( pitch ); + // Serial.print (" "); + // Serial.print ( "Accelerometer_Pitch:" ); + // Serial.print ( acc_pitch ); + // Serial.print (" "); + // Serial.print ( "," ); + // Serial.println ( gz ); + // Serial.print ( "," ); + // Serial.println ( gt ); + // Serial.print ( " " ); + // Serial.println ( acc_pitch); // Remote control - // Serial.print("ch1:"); - // Serial.print(pwm_time_ch1); - // Serial.print(" "); - // Serial.print("ch2:"); - // Serial.print(pwm_time_ch2); - // Serial.print("ch1mapped:"); - // Serial.print(rem_turn_speed_ref); - // Serial.print(" "); - // Serial.print("ch2mapped:"); - // Serial.println(rem_speed_ref); + // Serial.print("ch1:"); + // Serial.print(pwm_time_ch1); + // Serial.print(" "); + // Serial.print("ch2:"); + // Serial.print(pwm_time_ch2); + // Serial.print("ch1mapped:"); + // Serial.print(rem_turn_speed_ref); + // Serial.print(" "); + // Serial.print("ch2mapped:"); + // Serial.println(rem_speed_ref); // Encoders - // Serial.print("m1Raw:"); - // Serial.print(m1Raw); - // Serial.print(" "); - // Serial.print("m2Raw:"); - // Serial.println(m2Raw); + // Serial.print("m1Raw:"); + // Serial.print(m1Raw); + // Serial.print(" "); + // Serial.print("m2Raw:"); + // Serial.println(m2Raw); -// Motors - // Serial.print("SpeedControllerOut:"); - // Serial.print(SC_cont_out); - // Serial.print(" "); - // Serial.print("BalanceOLControllerOut:"); - // Serial.print(OL_cont_out); - // Serial.print(" "); - // Serial.print("BalanceILControllerOut:"); - // Serial.print(IL_cont_out); - // Serial.print(" "); - // Serial.print("TurnControllerOut:"); - // Serial.println(TC_cont_out); - // Serial.print(" "); - // Serial.print("M1_CMD:"); - // Serial.print(M1_Speed_CMD); - // Serial.print(" "); - // Serial.print("M2_CMD:"); - // Serial.println(M2_Speed_CMD); + // Motors + // Serial.print("SpeedControllerOut:"); + // Serial.print(SC_cont_out); + // Serial.print(" "); + // Serial.print("BalanceOLControllerOut:"); + // Serial.print(OL_cont_out); + // Serial.print(" "); + // Serial.print("BalanceILControllerOut:"); + // Serial.print(IL_cont_out); + // Serial.print(" "); + // Serial.print("TurnControllerOut:"); + // Serial.println(TC_cont_out); + // Serial.print(" "); + // Serial.print("M1_CMD:"); + // Serial.print(M1_Speed_CMD); + // Serial.print(" "); + // Serial.print("M2_CMD:"); + // Serial.println(M2_Speed_CMD); - // Serial.print("M1_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][0]); - // Serial.print(" "); - // 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]); + // Serial.print("M1_Ang_Vel:"); + // Serial.print(motor_ang_vel[0][0]); + // Serial.print(" "); + // 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]); // //PS3 Controller // if (Ps3.isConnected()) {