diff --git a/Main/IMU.ino b/Main/IMU.ino index 83ff38b..e75255d 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -7,7 +7,7 @@ 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 gx, gy, gz; float gt; float acc_pitch; float pitch_rate; @@ -16,26 +16,30 @@ float pitch_prev = 0; void readIMU() { + // Serial.println("ReadingIMU"); //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); + int* accelerometerReadings = IMU.readFromAccelerometer(); + ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value); //Magnetometer - cx = IMU.compass_x(IMU.readFromCompass()); - cy = IMU.compass_y(IMU.readFromCompass()); - cz = IMU.compass_z(IMU.readFromCompass()); + int* compassReadings = IMU.readFromCompass(); + cx = IMU.compass_x(compassReadings); + cy = IMU.compass_y(compassReadings); + cz = IMU.compass_z(compassReadings); // 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 + float* gyroReadings = IMU.readGyro(); + gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate //Temperature sensor - gt = IMU.temp(IMU.readGyro()); + gt = IMU.temp(gyroReadings); // Pitch angle from accelerometer diff --git a/Main/Main.ino b/Main/Main.ino index 7a2c05c..adb3cef 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -49,11 +49,14 @@ void setup() { delay(10); //Initialice I2C - Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); - delay(10); + //Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); + //delay(10); //Initialize IMU - IMU.init(); + Serial.println("Before IMU init"); + IMU.init(IMU_I2C_SCL, IMU_I2C_SDA); + //IMU.init(); + Serial.println("After IMU init"); delay(10); //Initialize encoder interrupts @@ -84,6 +87,7 @@ void setup() { } void loop() { + // Serial.println("Loop"); //Update time variables tNow = micros(); dT = tNow - tLast; //[Cycle time in microseconds] @@ -91,7 +95,7 @@ void loop() { //Get sensor data - readIMU(); + //readIMU(); //Control motors @@ -99,7 +103,7 @@ void loop() { // Plot - //plot(); + plot(); //Save time for next cycle diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 60694ab..743be63 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -28,7 +28,7 @@ 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; - +float speedCmd1, speedCmd2; //Matrices @@ -84,14 +84,18 @@ void motors() { M2_Speed_CMD = IL_cont_out + TC_cont_out; //Sum speed command for motors - M1_Speed_CMD = 0; - M2_Speed_CMD = 0; + speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); + M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; + motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); + M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); //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 = 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; //Update variables for next scan cycle m1RawLast = m1Raw; diff --git a/Main/plot.ino b/Main/plot.ino index 5217571..7a9c157 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -9,7 +9,7 @@ void plot() { // IMU // Serial.print ( "Pitch:" ); - // Serial.print ( pitch ); + // Serial.println ( pitch ); // Serial.print (" "); // Serial.print ( "Accelerometer_Pitch:" ); // Serial.print ( acc_pitch ); @@ -52,12 +52,15 @@ void plot() { // Serial.print("BalanceILControllerOut:"); // Serial.print(IL_cont_out); // Serial.print(" "); - // Serial.print("TurnControllerOut:"); - // Serial.println(TC_cont_out); + // Serial.print("SpeedCmd1:"); + // Serial.println(speedCmd1); // Serial.print(" "); // Serial.print("M1_CMD:"); // Serial.print(M1_Speed_CMD); // Serial.print(" "); + // Serial.print("SpeedCmd2:"); + // Serial.println(speedCmd2); + // Serial.print(" "); // Serial.print("M2_CMD:"); // Serial.println(M2_Speed_CMD); diff --git a/README.md b/README.md index f23c65d..640f94a 100644 --- a/README.md +++ b/README.md @@ -1 +1,15 @@ -# Balancebot +# BalanceBot + +## Arduino Board Settings +My board has this etched on it: ESP-WROOM-32 + +These settings allow upload: +* Board: FireBeetle-ESP32 + + +## Dependencies +[Ps3Controller.h](https://github.com/jvpernis/esp32-ps3) + +[MatrixMath.h](https://github.com/eecharlie/MatrixMath) + +[GY_85.h](https://github.com/sqrtmo/GY-85-arduino/tree/master)