quicksave

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);
+  int* accelerometerReadings = IMU.readFromAccelerometer();
-  ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value);
+  ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value);
-  az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), 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());
+  int* compassReadings = IMU.readFromCompass();
-  cy = IMU.compass_y(IMU.readFromCompass());
+  cx = IMU.compass_x(compassReadings);
-  cz = IMU.compass_z(IMU.readFromCompass());
+  cy = IMU.compass_y(compassReadings);
+  cz = IMU.compass_z(compassReadings);
 
 
   //  Gyrocope
-  gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value);  //  gx -  Pitch rate
+  float* gyroReadings = IMU.readGyro();
-  gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value);  //  gy -  Roll rate
+  gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value);  //  gx -  Pitch rate
-  gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value);  //  gz -  Yaw 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

Main/Main.ino

@@ -49,11 +49,14 @@ void setup() {
   delay(10);
 
   //Initialice I2C
-  Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
+  //Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA);
-  delay(10);
+  //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

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;
+  speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0);
-  M2_Speed_CMD = 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 = 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 + 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;

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.print("SpeedCmd1:");
-  // Serial.println(TC_cont_out);
+  // 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);
 

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)