diff --git a/Main/IMU.ino b/Main/IMU.ino index 155b71a..400f381 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -1,9 +1,22 @@ -//IMU variables -int ax_, ay_, az_; -int ax, ay, az; -int cx, cy, cz; -float gx, gy, gz, gt; -float acc_pitch; +//CONSTANTS +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 ax, ay, az; +int cx_, cy_, cz_; +int gx_, gy_, gz_; +int gx, gy, gz; +//float gt; +float acc_pitch; +float pitch_rate; +float dAngle, estAngle; +float pitch = 0; +float pitch_prev = 0; + void readIMU() { ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() ); @@ -14,51 +27,92 @@ void readIMU() { // cy = IMU.compass_y( IMU.readFromCompass() ); // cz = IMU.compass_z( IMU.readFromCompass() ); - gx = IMU.gyro_x( IMU.readGyro() ); - gy = IMU.gyro_y( IMU.readGyro() ); - gz = IMU.gyro_z( IMU.readGyro() ); + gx_ = IMU.gyro_x( IMU.readGyro() ); + gy_ = IMU.gyro_y( IMU.readGyro() ); + gz_ = IMU.gyro_z( IMU.readGyro() ); // 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. - if (ax_ > 60000) { - ax = (ax_ - 65535); + //Convert accelerometer + if (ax_ > 32768) { + ax = (ax_ - acc_overflow_value); } else { ax = ax_; } - if (ay_ > 60000) { - ay = (ay_ - 65535); + if (ay_ > 32768) { + ay = (ay_ - acc_overflow_value); } else { ay = ay_; } - if (az_ > 60000) { - az = (az_ - 65535); + if (az_ > 32768) { + az = (az_ - acc_overflow_value); } else { az = az_; } - acc_pitch = -1*atan2(ax,sqrt((pow(ay,2)+pow(az,2))))*180/3.14; - -//Serial plotter -// map(ax,-140,140,-1,1) -// map(ay,-140,140,-1,1) -// map(az,-140,140,-1,1) + //Convert gyro + // Gyroscope coordinate system + // gx - Pitch rate + // gy - Roll rate + // gz - Yaw rate -// Serial.print ( " x:" ); -// Serial.print ( ax ); -// Serial.print ( "," ); -// Serial.print ( ay ); -// Serial.print ( "," ); -// Serial.println ( az ); -// Serial.print ( "," ); -// Serial.println ( acc_pitch); + // 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_; + } + // Pitch angle from accelerometer + acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI; + + //Pitch rate from gyroscope + pitch_rate = -gx; + + + //Complementary filter + dAngle = pitch_rate * dT * pow(10, -6); + pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha); + pitch_prev = pitch; + + + //Serial plotter +// 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); } diff --git a/Main/Main.ino b/Main/Main.ino index e83c16f..7b462b1 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -1,3 +1,5 @@ +#include + //Import #include "GY_85.h" #include @@ -22,6 +24,8 @@ const byte IMU_I2C_SDA = 27; //System variables unsigned long tNow = micros(); +unsigned long tLast = micros()+13000; +int dT = 0; //Motor variables @@ -156,6 +160,8 @@ 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 @@ -188,13 +194,14 @@ 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 - tNow = micros(); + tNow = micros(); + dT = tNow - tLast; //[Cycle time in microseconds] //// //Only print encoder value if value changed since last print // if (m1Raw != m1RawLast) { @@ -212,7 +219,7 @@ void loop() { //Act motorControl(); - + 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 dd42583..acb1c7e 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -1,16 +1,17 @@ //Constants const float wheel_diameter = 0.067708; const float pulses_per_turn = 1320.0; +const float filt_gain = 15; //Tuning -const float K = 2.0; -const float I = 10.0; +const float K = 3.5; +const float I = 7.5; //Help variables -int dEnc, dT; -float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered; -unsigned long tLast; +int dEnc; +float dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered; +float dVel, diff; -float speed_setp, referenceSpeed, actualSpeed; +float speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered; float error, iError; int speedCMD; @@ -18,9 +19,10 @@ int speedCMD; void motorControl() { //Speed Controller - referenceSpeed = acc_pitch * ((4096.0) / (90.0)); + referenceSpeed = pitch * ((4096.0) / (90.0)); actualSpeed = lin_vel * 4096.0; - error = referenceSpeed - actualSpeed; + actualSpeedFiltered = lin_vel_filtered * 4096.0; + error = referenceSpeed - actualSpeedFiltered; iError = iError + (error * dT * pow(10, -6) * I); speedCMD = round((error * K) + iError); @@ -46,13 +48,7 @@ void motorControl() { ledcWrite(1, -1 * speedCMD); ledcWrite(2, 0); } - // Serial.print (acc_pitch); - // Serial.print ( "," ); - // Serial.println(speed_setp); - // - // ledcWrite(1, 0); - // ledcWrite(2, 0); //Motor 2 @@ -60,26 +56,45 @@ void motorControl() { // 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] - dT = tNow - tLast; //[Cycle time in microseconds] + 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); - Serial.print(referenceSpeed); - Serial.print ( "," ); - Serial.print(actualSpeed); - Serial.print ( "," ); - Serial.println(speedCMD); - // Serial.print ( "," ); + // 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); + + + // Serial plotter + Serial.print("referenceSpeed:"); + Serial.print(referenceSpeed * (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(" "); + Serial.print("speedCMD:"); + Serial.println(speedCMD * (100.0 / 4096.0)); + // Serial.print ( "," ); // Serial.println(lin_vel); // Serial.print ( "," ); //Update variables for next scan cycle m1RawLast = m1Raw; - tLast = tNow; + }