From 72c4f2d5911b5a1276719a4c6454705928a85b4e Mon Sep 17 00:00:00 2001 From: Stedd Date: Fri, 20 Dec 2019 20:03:48 +0100 Subject: [PATCH] Functions --- Main/IMU.ino | 78 ++++++++------------- Main/Main.ino | 48 ++++++------- Main/motorControl.ino | 153 +++++++++++++++++++++--------------------- 3 files changed, 126 insertions(+), 153 deletions(-) diff --git a/Main/IMU.ino b/Main/IMU.ino index 400f381..9362c9b 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -33,28 +33,10 @@ void readIMU() { // gt = IMU.temp ( IMU.readGyro() ); - //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. //Convert accelerometer - if (ax_ > 32768) { - ax = (ax_ - acc_overflow_value); - } - else { - ax = ax_; - } - - if (ay_ > 32768) { - ay = (ay_ - acc_overflow_value); - } - else { - ay = ay_; - } - - if (az_ > 32768) { - az = (az_ - acc_overflow_value); - } - else { - az = az_; - } + ax = convertInt(ax_, acc_overflow_value); + ay = convertInt(ay_, acc_overflow_value); + az = convertInt(az_, acc_overflow_value); //Convert gyro @@ -62,30 +44,11 @@ void readIMU() { // gx - Pitch rate // gy - Roll rate // gz - Yaw rate - // Gyro is calibrated for +-2000deg/s // Conversion is happening in GY_85.h line 48-50 - - if (gx_ > 2279) { - gx = (gx_ - gyro_overflow_value); - } - else { - gx = gx_; - } - - if (gy_ > 2279) { - gy = (gy_ - gyro_overflow_value); - } - else { - gy = gy_; - } - - if (gz_ > 2279) { - gz = (gz_ - gyro_overflow_value); - } - else { - gz = gz_; - } + gx = convertInt(gx_, gyro_overflow_value); + gy = convertInt(gy_, gyro_overflow_value); + gz = convertInt(gz_, gyro_overflow_value); // Pitch angle from accelerometer @@ -103,12 +66,12 @@ void readIMU() { //Serial plotter -// Serial.print ( "Pitch:" ); -// Serial.print ( pitch ); -// Serial.print (" "); -// Serial.print ( "Accelerometer_Pitch:" ); -// Serial.print ( acc_pitch ); -// Serial.print (" "); + // 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 ( "," ); @@ -116,3 +79,20 @@ void readIMU() { // Serial.print ( "," ); // Serial.println ( acc_pitch); } + + +int convertInt(int raw, int overflow_value_) { + //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. + int conv_val; + + if (raw > (overflow_value_ / 2)) { + conv_val = (raw - overflow_value_); + } + + else { + conv_val = raw; + } + + return conv_val; + +} diff --git a/Main/Main.ino b/Main/Main.ino index 7b462b1..aa34146 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -1,7 +1,5 @@ -#include - //Import -#include "GY_85.h" +#include #include @@ -10,21 +8,21 @@ GY_85 IMU; //GPIO PIN MAPPING -const byte M1_ENC_A = 34; -const byte M1_ENC_B = 35; -const byte M2_ENC_A = 32; -const byte M2_ENC_B = 33; -const byte M1_A = 18; -const byte M1_B = 19; -const byte M2_A = 16; -const byte M2_B = 17; -const byte IMU_I2C_SCL = 26; -const byte IMU_I2C_SDA = 27; +const byte M1_ENC_A = 34; +const byte M1_ENC_B = 35; +const byte M2_ENC_A = 32; +const byte M2_ENC_B = 33; +const byte M1_A = 18; +const byte M1_B = 19; +const byte M2_A = 16; +const byte M2_B = 17; +const byte IMU_I2C_SCL = 26; +const byte IMU_I2C_SDA = 27; -//System variables +//Time variables unsigned long tNow = micros(); -unsigned long tLast = micros()+13000; +unsigned long tLast = micros() + 13000; int dT = 0; @@ -194,32 +192,28 @@ void setup() { ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); -// Serial.println("Reference,Actual,SpeedCommand"); + // Serial.println("Reference,Actual,SpeedCommand"); } void loop() { - //Update system variables + //Update time variables tNow = micros(); dT = tNow - tLast; //[Cycle time in microseconds] - //// //Only print encoder value if value changed since last print - // if (m1Raw != m1RawLast) { - // Serial.println(m1Raw); - // m1RawLast = m1Raw; - // } - //Sense + //Get sensor data readIMU(); - //Think + //Control motor + motors(); - //Act - motorControl(); - + //Save time tLast = tNow; + + //Delay delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz } diff --git a/Main/motorControl.ino b/Main/motorControl.ino index acb1c7e..7abcd54 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -1,100 +1,99 @@ //Constants -const float wheel_diameter = 0.067708; -const float pulses_per_turn = 1320.0; -const float filt_gain = 15; +const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION)); +const float WHEEL_DIAMETER = 0.067708; +const float PULSES_PER_TURN = 1320.0; + //Tuning -const float K = 3.5; -const float I = 7.5; +const float K = 3.5; +const float I = 7.5; +const float filter_gain = 15; //Help variables -int dEnc; -float dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered; -float dVel, diff; +float M1_Lin_Vel, M2_Lin_Vel; +int M1_Speed_CMD, M2_Speed_CMD; +float M1_iError, M2_iError; -float speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered; -float error, iError; -int speedCMD; +float ref, act, error; + +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); + +// ref = pitch * ((4096.0) / (90.0)); +// act = M2_Lin_Vel * 4096.0; +// error = ref - act; +// M2_iError = M2_iError + (error * dT * pow(10, -6) * I); +// M2_Speed_CMD = round((error * K) + M2_iError); -void motorControl() { - - //Speed Controller - referenceSpeed = pitch * ((4096.0) / (90.0)); - actualSpeed = lin_vel * 4096.0; - actualSpeedFiltered = lin_vel_filtered * 4096.0; - error = referenceSpeed - actualSpeedFiltered; - iError = iError + (error * dT * pow(10, -6) * I); - speedCMD = round((error * K) + iError); - - - // Speed command saturation - if (speedCMD > 4096) { - speedCMD = 4096; - } - else if (speedCMD < -4096) { - speedCMD = -4096; - } - else { - speedCMD = speedCMD; - } - - - //Motor 1 Control - if (speedCMD > 0) { - ledcWrite(1, 0); - ledcWrite(2, speedCMD); - } - else if (speedCMD < 0) { - ledcWrite(1, -1 * speedCMD); - ledcWrite(2, 0); - } - - - - //Motor 2 - // ledcWrite(3, 255); - // ledcWrite(4, 255); - - - //No speed command - // ledcWrite(1, 0); - // ledcWrite(2, 0); - ledcWrite(3, 0); - ledcWrite(4, 0); //Calculate speed from encoders - dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle] + 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); - dTurn = dEnc / pulses_per_turn; //[Amount wheel turned this cycle. 1 = full rotation] - ang_vel = (dTurn * 2 * PI) / (dT * 0.000001); - lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001); + //Motor 1 + motorControl(1, M1_Speed_CMD, MOTOR_SATURATION); - // Lowpass filter - // lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered)*(1-abs(diff))* dT * pow(10, -6)*filt_gain); - lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered) * dT * pow(10, -6) * filt_gain); + //Motor 2 +// motorSpeed(2, M2_Speed_CMD, MOTOR_SATURATION); + motorControl(2, 0, MOTOR_SATURATION); // Serial plotter - Serial.print("referenceSpeed:"); - Serial.print(referenceSpeed * (100.0 / 4096.0)); + Serial.print("M1_Speed_REF:"); + Serial.print(ref * (100.0 / 4096.0)); Serial.print(" "); -// Serial.print("actualSpeed:"); -// Serial.print(actualSpeed * (100.0 / 4096.0)); -// Serial.print(" "); - Serial.print("actualSpeedFiltered:"); - Serial.print(actualSpeedFiltered * (100.0 / 4096.0)); + Serial.print("M1_Speed_ACT:"); + Serial.print(act * (100.0 / 4096.0)); Serial.print(" "); - Serial.print("speedCMD:"); - Serial.println(speedCMD * (100.0 / 4096.0)); - // Serial.print ( "," ); - // Serial.println(lin_vel); - // Serial.print ( "," ); + Serial.print("M1_Speed_CMD:"); + Serial.println(M1_Speed_CMD * (100.0 / 4096.0)); //Update variables for next scan cycle m1RawLast = m1Raw; - - + m2RawLast = m2Raw; +} + +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 dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation] + float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001); + return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_); +} + +void motorControl(byte motorID, int speedCMD_, int saturation) { + //Calculate channel + byte ch1 = motorID * 2 - 1; + byte ch2 = motorID * 2; + + + // Speed command saturation + if (speedCMD_ > saturation) { + speedCMD_ = saturation; + } + else if (speedCMD_ < -saturation) { + speedCMD_ = -saturation; + } + else { + speedCMD_ = speedCMD_; + } + + + //Motor Control + if (speedCMD_ > 0) { + ledcWrite(ch1, 0); + ledcWrite(ch2, speedCMD_); + } + else if (speedCMD_ < 0) { + ledcWrite(ch1, -1 * speedCMD_); + ledcWrite(ch2, 0); + } }