1-Axis Attitude Control Module with ATOM

http://www.kenkai.jaxa.jp/research/innovation/triaxial.html

Since a small 3-axis attitude control module like JAXA is impossible at all. We aim to realize a 1-axis attitude control module (inverted pendulum).

In the future, we would like to implement it on the smart shoe fortune-telling device we made earlier. When I throw my shoe in the air, I want to receive tomorrow's weather data at that point and use the reaction wheel to control the landing posture of the shoe.

The model number of the motor was ID-549XW, but there is almost no information.

There are some parts that you cannot read... I tried it with trial and error on an actual machine.

Encoder operation

In the picture above, I could predict that the A-phase and B-phase are the output of the optical encoder and the third blue line from the left is the logic voltage, so I observed it with ADALM2000.

The wiring status is as follows.

First from left (Signal A phase): Oscilloscope 1 Ch

Second from left (phase B of signal): Oscilloscope 2 Ch

Third from the left (logic power supply): 5V applied

7th from left: GND

Phase A Phase B is like this

I also understood the logic power supply.

Rotation motion

I didn't know how to rotate it, but I finally confirmed the rotation by applying 6V or more to the motor power supply and applying the PWM signal.

Speed control with PWM input

High speed at 0V (Duty 0%), low speed at 5V (Duty 100%) or stop

The wiring status is as follows.

First from left (Signal A phase): Oscilloscope 1 Ch

Second from left (phase B of signal): Oscilloscope 2 Ch

Third from the left (logic power supply): 5V applied

Fourth from the left (rotation control): open clockwise, pulldown counterclockwise

5th from the left (PWM input): 20kHz, ON Duty 0% at maximum speed, 100% stop

6th from left (brake): Pull down to stop rotation

7th from left: GND

8th from left (motor power supply): 12V applied

The wiring between the motor and ATOM Matrix is as follows.

First from left (Signal A phase): G22

Second from left (phase B of signal): G19

Third from the left (logic power supply): 3.3V

7th from left: GND

Arduino code

Connect the A-phase and B-phase terminals of the rotary encoder to the ATOM Matrixn pins respectively and interrupt the output change to detect the rotation speed.

I am using the code introduced in the following article to observe the rotational position.

#include <FastLED.h>
#include <FastLED_NeoMatrix.h>
#include <Fonts/Org_01.h>
const int stepsPerRotate = 100;
int rot = 0, rotOld = -1;
#define ENC_A 22
#define ENC_B 19
volatile byte pos;
volatile int enc_count = 0;
#define PIN 27
#define mw 5
#define mh 5
#define NUMMATRIX (mw*mh)
#define Bright 20
CRGB matrixleds[NUMMATRIX];
FastLED_NeoMatrix *matrix = new FastLED_NeoMatrix(matrixleds, mw, mh,
NEO_MATRIX_TOP + NEO_MATRIX_LEFT +
NEO_MATRIX_ROWS + NEO_MATRIX_PROGRESSIVE );
int ledR = 255;
int ledG = 0;
int ledB = 0;
String text[10] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9"};
void setup() {
Serial.begin(115200);
pinMode(ENC_A, INPUT);
pinMode(ENC_B, INPUT);
attachInterrupt(ENC_A, ENC_READ, CHANGE);
attachInterrupt(ENC_B, ENC_READ, CHANGE);
matrix->setFont(&Org_01);
FastLED.addLeds<NEOPIXEL,PIN>(matrixleds, NUMMATRIX);
matrix->begin();
matrix->setTextWrap(false);
matrix->setBrightness(Bright);
matrix->setTextColor(matrix->Color(ledR, ledG, ledB));
}
void loop() {
rot = enc_count / stepsPerRotate;
if(rot != rotOld && rot >= 0 && rot < 10){
Serial.println(rot);
rotOld = rot;
matrix->fillScreen(0);
matrix->setCursor(0, 4);
matrix->print(text[rot]);
matrix->show();
}
}
void ENC_READ() {
byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);
byte old = pos & B00000011;
byte dir = (pos & B00110000) >> 4;
if (cur == 3) cur = 2;
else if (cur == 2) cur = 3;
if (cur != old)
{
if (dir == 0)
{
if (cur == 1 || cur == 3) dir = cur;
} else {
if (cur == 0)
{
if (dir == 1 && old == 3) enc_count--;
else if (dir == 3 && old == 1) enc_count++;
dir = 0;
}
}
bool rote = 0;
if (cur == 3 && old == 0) rote = 0;
else if (cur == 0 && old == 3) rote = 1;
else if (cur > old) rote = 1;
pos = (dir << 4) + (old << 2) + cur;
}
}

Here, I tried to control the rotation operation of the motor with M5 Stack ATOM Matrix.

Brushless motor ID-549XW

1st red from left: Motor power 12V applied (rotates at 6V or more)

2nd black from left: GND

3rd yellow from the left: Brake pulldown stops rotation

4th green from left: PWM input 20kHz, ON Duty 0% at maximum speed, 100% stop

5th from left Blue: Rotation control Open clockwise and pulldown counterclockwise

6th from left White: 3.3V or 5V

7th orange from the left: Encoder output B phase

8th from left Purple: Encoder output A phase

The variable resistor controls the rotation of the motor.

The resistance value is detected by G32 pin of ATOM Matrix.

The G26 pin of ATOM Matrix outputs a 20kHz PWM to control the rotation speed of the motor.

The brake and rotation direction of the motor are controlled by the tact switch.

Arduino code

#include "M5Atom.h"
#define ADC_pin 32
#define PWM_pin 26
int val, pwmDuty;
void setup() {
M5.begin(true, false, true);
ledcSetup(0, 20000, 8);
ledcAttachPin(PWM_pin, 0);
}
void loop() {
val = analogRead(ADC_pin);
pwmDuty = map(val, 0, 4095, 255, 0);
Serial.println(pwmDuty);
ledcWrite(0, pwmDuty);
delay(50);
}

Here, we are using a Kalman filter to realize a one-axis attitude control module.

The 6-axis inertial sensor MPU6886 mounted on the M5Stack ATOM Matrix derives the attitude angle.

Arduino code

#include "M5Atom.h"
#include <Ticker.h>
float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;
bool IMU6886Flag = false;
// Accelerometer offset
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
// Gyro sensor offset
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;
Ticker tickerUpdate;
//Sensor variation acquisition variable
int sample_num = 100;
int meas_interval = 10;
float theta_deg = 0.0;
float theta_dot = 0.0;
float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;
//=========================================================
//Kalman filter variables
//=========================================================
//Kalman filtering
float theta_update_freq = 400; //Hz
float theta_update_interval = 1.0/double(theta_update_freq); //2.5msec
//State vector x
//[[theta(degree)], [offset of theta_dot(degree/sec)]]
//Predicted value of state vector
float theta_data_predict[2][1];
float theta_data[2][1];
//Covariance matrix
float P_theta_predict[2][2];
float P_theta[2][2];
//"A" in the equation of state
float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};
//"B" in the equation of state
float B_theta[2][1] = {{theta_update_interval}, {0}};
//"C" in the output equation
float C_theta[1][2] = {{1, 0}};
//=========================================================
// Matrix operation function
//=========================================================
//Matrix sum
void mat_add(float *m1, float *m2, float *sol, int row, int column) {
for(int i=0; i<row; i++) {
for(int j=0; j<column; j++) {
sol[i*column + j] = m1[i*column + j] + m2[i*column + j];
}
}
return;
}
//Matrix difference
void mat_sub(float *m1, float *m2, float *sol, int row, int column){
for(int i=0; i<row; i++) {
for(int j=0; j<column; j++) {
sol[i*column + j] = m1[i*column + j] - m2[i*column + j];
}
}
return;
}
//Matrix multiplication
void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2){
for(int i=0; i<row1; i++){
for(int j=0; j<column2; j++){
sol[i*column2 + j] = 0;
for(int k=0; k<column1; k++) {
sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];
}
}
}
return;
}
//Transpose matrix calculation
void mat_tran(float *m1, float *sol, int row_original, int column_original) {
for(int i=0; i<row_original; i++) {
for(int j=0; j<column_original; j++) {
sol[j*row_original + i] = m1[i*column_original + j];
}
}
return;
}
//Matrix constant multiplication
void mat_mul_const(float *m1,float c, float *sol, int row, int column){
for(int i=0; i<row; i++){
for(int j=0; j<column; j++){
sol[i*column + j] = c * m1[i*column + j];
}
}
return;
}
//Inverse matrix calculation
void mat_inv(float *m, float *sol, int column, int row){
//allocate memory for a temporary matrix
float* temp = (float *)malloc( column*2*row*sizeof(float) );
//make the augmented matrix
for(int i=0; i<column; i++) {
//copy original matrix
for(int j=0; j<row; j++) {
temp[i*(2*row) + j] = m[i*row + j];
}
//make identity matrix
for(int j=row; j<row*2; j++) {
if(j-row == i) {
temp[i*(2*row) + j] = 1;
}
else {
temp[i*(2*row) + j] = 0;
}
}
}
//Sweep (down)
for(int i=0; i<column; i++) {
//pivot selection
float pivot = temp[i*(2*row) + i];
int pivot_index = i;
float pivot_temp;
for(int j=i; j<column;j++) {
if( temp[j*(2*row)+i] > pivot ) {
pivot = temp[j*(2*row) + i];
pivot_index = j;
}
}
if(pivot_index != i) {
for(int j=0; j<2*row; j++) {
pivot_temp = temp[ pivot_index * (2*row) + j ];
temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];
temp[i*(2*row) + j] = pivot_temp;
}
}
//division
for(int j=0; j<2*row; j++) {
temp[i*(2*row) + j] /= pivot;
}
//sweep
for(int j=i+1; j<column; j++) {
float temp2 = temp[j*(2*row) + i];
//sweep each row
for(int k=0; k<row*2; k++) {
temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];
}
}
}
//Sweep (up)
for(int i=0; i<column-1; i++) {
for(int j=i+1; j<column; j++) {
float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];
for(int k=0; k<2*row; k++) {
temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];
}
}
}
//copy result
for(int i=0; i<column; i++) {
for(int j=0; j<row; j++) {
sol[i*row + j] = temp[i*(2*row) + (j+row)];
}
}
free(temp);
return;
}
//Sensor offset calculation
void offset_cal(){
delay(1000);
accXoffset = 0;
accYoffset = 0;
accZoffset = 0;
gyroXoffset = 0;
gyroYoffset = 0;
gyroZoffset = 0;
for(int i=0; i<10; i++) {
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
delay(meas_interval);
accXoffset += accX;
accYoffset += accY;
accZoffset += accZ;
gyroXoffset += gyroX;
gyroYoffset += gyroY;
gyroZoffset += gyroZ;
}
accXoffset /= 10;
accYoffset /= 10;
accZoffset = accZoffset / 10 + 1.0;
gyroXoffset /= 10;
gyroYoffset /= 10;
gyroZoffset /= 10;
}
//Inclination data acquisition from acceleration sensor [deg]
float get_acc_data() {
M5.IMU.getAccelData(&accX,&accY,&accZ);
//Use the obtained sensor value after subtracting the offset
//Inclination angle derivation unit is deg
theta_deg = atan(-(accX - accXoffset) / (accZ - accZoffset)) * 57.29578f;
return theta_deg;
}
//Measurement of inclination variation using an acceleration sensor
void acc_init(){
float theta_array[sample_num];
for(int i=0; i<sample_num; i++) {
theta_array[i] = get_acc_data();
delay(meas_interval);
}
//Average value
theta_mean = 0;
for(int i=0; i<sample_num; i++) {
theta_mean += theta_array[i];
}
theta_mean /= sample_num;
//Dispersion
float temp;
theta_variance = 0;
for(int i=0; i<sample_num; i++) {
temp = theta_array[i] - theta_mean;
theta_variance += temp*temp;
}
theta_variance /= sample_num;
}
//Y-axis angular velocity acquisition
float get_gyro_data() {
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
//Use the obtained sensor value after subtracting the offset
theta_dot = gyroY - gyroYoffset;
return theta_dot;
}
//Gyro sensor variation measurement
void gyro_init() {
float theta_dot_array[sample_num];
for(int i=0;i<sample_num;i++) {
theta_dot_array[i] = get_gyro_data();
delay(meas_interval);
}
//Average value
theta_dot_mean = 0;
for(int i=0;i<sample_num;i++) {
theta_dot_mean += theta_dot_array[i];
}
theta_dot_mean /= sample_num;
//Dispersion
float temp;
theta_dot_variance = 0;
for(int i=0; i<sample_num; i++) {
temp = theta_dot_array[i] - theta_dot_mean;
theta_dot_variance += temp*temp;
}
theta_dot_variance /= sample_num;
}
//=========================================================
//Kalman filter algorithm processing
//=========================================================
void update_theta()
{
//Angle measurement by accelerometer
float y = get_acc_data(); //degree
//Input data: Angular velocity
float theta_dot_gyro = get_gyro_data(); //degree/sec
//Kalman gain calculation: G = P'C^T(W+CP'C^T)^-1
float P_CT[2][1] = {};
float tran_C_theta[2][1] = {};
mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T
mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T
float G_temp1[1][1] = {};
mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T
float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1
float G1[2][1] = {};
mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1
//Estimated tilt angle calculation: theta = theta'+G(y-Ctheta')
float C_theta_theta[1][1] = {};
mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta'
float delta_y = y - C_theta_theta[0][0];//y-Ctheta'
float delta_theta[2][1] = {};
mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1);
mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);
//Covariance matrix calculation: P=(I-GC)P'
float GC[2][2] = {};
float I2[2][2] = {{1,0},{0,1}};
mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC
float I2_GC[2][2] = {};
mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC
mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P'
//Calculation of the predicted value of the tilt angle at the next time: theta'=Atheta+Bu
float A_theta_theta[2][1] = {};
float B_theta_dot[2][1] = {};
mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta
mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu
mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu
//Next time covariance matrix calculation: P'=APA^T + BUB^T
float AP[2][2] = {};
float APAT[2][2] = {};
float tran_A_theta[2][2] = {};
mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T
mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP
mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T
float BBT[2][2];
float tran_B_theta[1][2] = {};
mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T
mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T
float BUBT[2][2] = {};
mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T
mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T
}
//Initial setting of Kalman filter Assume 0° (upright) initial posture
void ini_theta(){
//Initial setting startLED ON
M5.dis.fillpix(0x00f000);
//Sensor offset calculation
offset_cal();
//Acquisition of sensor variability Measured 100 times to derive variance
acc_init();
gyro_init();
//Initial setting of Kalman filter Assume 0° (upright) initial posture
theta_data_predict[0][0] = 0;
theta_data_predict[1][0] = theta_dot_mean;
P_theta_predict[0][0] = 1;
P_theta_predict[0][1] = 0;
P_theta_predict[1][0] = 0;
P_theta_predict[1][1] = theta_dot_variance;
//Initial setting completed LED OFF
M5.dis.clear();
}
void setup() {
M5.begin(true, false, true);
if (M5.IMU.Init() != 0)
IMU6886Flag = false;
else
IMU6886Flag = true;
//Kalman filter initial settings
ini_theta();
//Angle derivation with Timer Kalman filter
tickerUpdate.attach(theta_update_interval, update_theta);
}
void loop() {
//Initialization
if (M5.Btn.wasPressed()){
ini_theta();
}
//Tilt angle by acceleration sensor
Serial.print(theta_deg);
Serial.print(", ");
//Kalman filter angle estimate
Serial.println(theta_data[0][0]);
//LED display
int line = map(theta_data[0][0], -30, 30, 4, 0);
if(line >= 0 && line < 5){
M5.dis.clear();
for(int i = 0; i < 5; i++){
M5.dis.drawpix(i * 5 + line, 0xf00000);
}
}
delay(50);
M5.update();
}

Here's an overview of what we do:

The offset value of the sensor is calculated by averaging 10 times and the measured value is used after subtracting the offset.

Sensor dispersion calculation: Acceleration and gyro sensor are measured 100 times to derive variance in advance

Kalman filter initialization: Initialization of matrix parameters used

The Kalman filter algorithm is activated by a timer interrupt (every 2.5 msec) to calculate the estimated tilt angle.

Serial output of tilt angle by accelerometer and angle estimate by Kalman filter every 50 msec

Indicator on M5Atom LED matrix in the range of -30° to 30°

Here, I would like to use the detected posture angle to control the brushless motor as a foothold for realizing a 1-axis posture control module.

The G26 pin of the ATOM Matrix outputs a 20kHz PWM to control the motor rotation speed.

Use the G32 pin to switch the rotation direction.

The motor is controlled based on the calculated angle.

The direction of rotation is switched depending on whether the posture angle is positive or negative.

Then, the rotation speed is increased in proportion to the size of the angle (P control).

#include "M5Atom.h"
#include <Ticker.h>

#define rote_pin 32
#define PWM_pin 26

int val, pwmDuty;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;
bool IMU6886Flag = false;

//Accelerometer offset
float accXoffset = 0, accYoffset = 0, accZoffset = 0;

//Gyro sensor offset
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;

Ticker tickerUpdate;


//Variable for acquisition of sensor variation
int sample_num = 100;
int meas_interval = 10;
float theta_deg = 0.0;
float theta_dot = 0.0;

float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;


//=========================================================
//Kalman filter variables
//=========================================================

//Kalman filtering
float theta_update_freq = 400; //Hz
float theta_update_interval = 1.0/double(theta_update_freq); //2.5msec
//State vector x
//[[theta(degree)], [offset of theta_dot(degree/sec)]]
//Predicted value of state vector
float theta_data_predict[2][1];
float theta_data[2][1];
//Covariance matrix
float P_theta_predict[2][2];
float P_theta[2][2];
//"A" in the equation of state
float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};
//"B" in the equation of state
float B_theta[2][1] = {{theta_update_interval}, {0}};
//"C" in the output equation
float C_theta[1][2] = {{1, 0}};


//=========================================================
// Matrix operation function
//=========================================================

//Matrix sum
void mat_add(float *m1, float *m2, float *sol, int row, int column) {
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] + m2[i*column + j];    
        }    
    }
    return;
}

//Matrix difference
void mat_sub(float *m1, float *m2, float *sol, int row, int column){
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] - m2[i*column + j];    
        }    
    }
    return;
}

//Matrix multiplication
void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2){
    for(int i=0; i<row1; i++){
        for(int j=0; j<column2; j++){
            sol[i*column2 + j] = 0;
            for(int k=0; k<column1; k++) {
                sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];    
            }
        }    
    }
    return;
}

//Transpose matrix calculation
void mat_tran(float *m1, float *sol, int row_original, int column_original) {
    for(int i=0; i<row_original; i++) {
        for(int j=0; j<column_original; j++) {
            sol[j*row_original + i] = m1[i*column_original + j];    
        }    
    }
    return;
}

//Matrix constant multiplication
void mat_mul_const(float *m1,float c, float *sol, int row, int column){
    for(int i=0; i<row; i++){
        for(int j=0; j<column; j++){
            sol[i*column + j] = c * m1[i*column + j];    
        }    
    }
    return;
}

//Inverse matrix calculation
void mat_inv(float *m, float *sol, int column, int row){
    //allocate memory for a temporary matrix
    float* temp = (float *)malloc( column*2*row*sizeof(float) );
    
    //make the augmented matrix
    for(int i=0; i<column; i++) {
        //copy original matrix
        for(int j=0; j<row; j++) {
            temp[i*(2*row) + j] = m[i*row + j];  
        }
        
        //make identity matrix
        for(int j=row; j<row*2; j++) {
            if(j-row == i) {
                temp[i*(2*row) + j] = 1;
            }    
            else {
                temp[i*(2*row) + j] = 0;    
            }
        }
    }

    //Sweep (down)
    for(int i=0; i<column; i++) {
        //pivot selection
        float pivot = temp[i*(2*row) + i];
        int pivot_index = i;
        float pivot_temp;
        for(int j=i; j<column;j++) {
            if( temp[j*(2*row)+i] > pivot ) {
                pivot = temp[j*(2*row) + i];
                pivot_index = j;
            }    
        }  
        if(pivot_index != i) {
            for(int j=0; j<2*row; j++) {
                pivot_temp = temp[ pivot_index * (2*row) + j ];
                temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];
                temp[i*(2*row) + j] = pivot_temp;    
            }    
        }
        
        //division
        for(int j=0; j<2*row; j++) {
            temp[i*(2*row) + j] /= pivot;    
        }
        
        //sweep
        for(int j=i+1; j<column; j++) {
            float temp2 = temp[j*(2*row) + i];
            
            //sweep each row
            for(int k=0; k<row*2; k++) {
                temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];    
            }    
        }
    }
        
    //Sweep (up)
    for(int i=0; i<column-1; i++) {
        for(int j=i+1; j<column; j++) {
            float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];   
            for(int k=0; k<2*row; k++) {
                temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];    
            }
        }    
    }     
    
    //copy result
    for(int i=0; i<column; i++) {
        for(int j=0; j<row; j++) {
            sol[i*row + j] = temp[i*(2*row) + (j+row)];    
        }    
    }
    free(temp);
    return;
}


//Sensor offset calculation
void offset_cal(){
  delay(1000);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;

  for(int i=0; i<10; i++) {
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(meas_interval);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  accXoffset /= 10;
  accYoffset /= 10;
  accZoffset = accZoffset / 10 + 1.0;
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}


//Inclination data acquisition from acceleration sensor [deg]
float get_acc_data() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //Use the obtained sensor value after subtracting the offset
  //Inclination angle derivation unit is deg
  theta_deg  = atan(-(accX - accXoffset) / (accZ - accZoffset)) * 57.29578f;
  return theta_deg;
}


//Measurement of inclination variation using an acceleration sensor
void acc_init(){
  float theta_array[sample_num];
  for(int i=0; i<sample_num; i++) {
    theta_array[i] = get_acc_data();
    delay(meas_interval);
  }

  //Average value
  theta_mean = 0;
  for(int i=0; i<sample_num; i++) {
    theta_mean += theta_array[i];
  }
  theta_mean /= sample_num;
    
  //Dispersion
  float temp;
  theta_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_array[i] - theta_mean;
    theta_variance += temp*temp;
  }
  theta_variance /= sample_num;
}


//Y-axis angular velocity acquisition
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  //Use the obtained sensor value after subtracting the offset
  theta_dot = gyroY - gyroYoffset;
  return theta_dot;
}

//Gyro sensor variation measurement
void gyro_init() {
  float theta_dot_array[sample_num];
  for(int i=0;i<sample_num;i++) {
    theta_dot_array[i] =  get_gyro_data();
    delay(meas_interval);
  }
    
  //Average value
  theta_dot_mean = 0;
  for(int i=0;i<sample_num;i++) {
    theta_dot_mean += theta_dot_array[i];    
  }
  theta_dot_mean /= sample_num;
 
  //Dispersion
  float temp;
  theta_dot_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_dot_array[i] - theta_dot_mean;
    theta_dot_variance += temp*temp;    
  }
  theta_dot_variance /= sample_num;
}


//=========================================================
//Kalman filter algorithm processing
//=========================================================
void update_theta()
{     
    //Angle measurement by accelerometer
    float y = get_acc_data(); //degree
    
    //Input data: Angular velocity
    float theta_dot_gyro = get_gyro_data(); //degree/sec
      
    //Kalman gain calculation: G = P'C^T(W+CP'C^T)^-1
    float P_CT[2][1] = {};
    float tran_C_theta[2][1] = {};
    mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T
    mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T
    float G_temp1[1][1] = {};
    mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T
    float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1
    float G1[2][1] = {};
    mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1
    
    //Estimated tilt angle calculation: theta = theta'+G(y-Ctheta')
    float C_theta_theta[1][1] = {};
    mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta'
    float delta_y = y - C_theta_theta[0][0];//y-Ctheta'
    float delta_theta[2][1] = {};
    mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1);
    mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);
           
    //Covariance matrix calculation: P=(I-GC)P'
    float GC[2][2] = {};
    float I2[2][2] = {{1,0},{0,1}};
    mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC
    float I2_GC[2][2] = {};
    mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC
    mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P'
      
    //Calculation of the predicted value of the tilt angle at the next time: theta'=Atheta+Bu
    float A_theta_theta[2][1] = {};
    float B_theta_dot[2][1] = {};
    mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta
    mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu
    mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu 
    
    //Next time covariance matrix calculation: P'=APA^T + BUB^T
    float AP[2][2] = {};   
    float APAT[2][2] = {};
    float tran_A_theta[2][2] = {};
    mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T 
    mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP
    mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T
    float BBT[2][2];
    float tran_B_theta[1][2] = {};
    mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T
    mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T
    float BUBT[2][2] = {};
    mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T
    mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T
}


//Initial setting of Kalman filter Assume 0° (upright) initial posture
void ini_theta(){
  //Initial setting start LED ON
  M5.dis.fillpix(0x00f000);

  //Sensor offset calculation
  offset_cal();
  
  //Acquisition of sensor variability Measured 100 times to derive variance
  acc_init();
  gyro_init();

  //Initial setting of Kalman filter Assume 0° (upright) initial posture
  theta_data_predict[0][0] = 0;
  theta_data_predict[1][0] = theta_dot_mean;
  
  P_theta_predict[0][0] = 1;
  P_theta_predict[0][1] = 0;
  P_theta_predict[1][0] = 0;
  P_theta_predict[1][1] = theta_dot_variance;

  //Initial setting completed LED OFF
  M5.dis.clear(); 
}


void setup() {
  M5.begin(true, false, true);

  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

  //Kalman filter initial settings
  ini_theta();

  //Angle derivation with Timer Kalman filter
  tickerUpdate.attach(theta_update_interval, update_theta); 

  ledcSetup(0, 20000, 8);
  ledcAttachPin(PWM_pin, 0);
  
  pinMode(rote_pin, OUTPUT);
}


void loop() {
  //Initialization
  if (M5.Btn.wasPressed()){
    ini_theta();
  }

  /*
  //Tilt angle by acceleration sensor
  Serial.print(theta_deg);
  Serial.print(", ");
  //Kalman filter angle estimate
  Serial.println(theta_data[0][0]);
  */

  //LED Display
  M5.dis.clear(); 
  if(theta_data[0][0] > 30.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5, 0x00f000);
    }
  }else if(theta_data[0][0] <= 30.0 && theta_data[0][0] > 20.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5, 0xf00000);
    }
  }else if(theta_data[0][0] <= 20.0 && theta_data[0][0] > 10.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 1, 0xf00000);
    }
  }else if(theta_data[0][0] <= 10.0 && theta_data[0][0] > 5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0xf00000);
    }
  }else if(abs(theta_data[0][0]) <= 5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0x00f000);
    }
  }else if(theta_data[0][0] >= -10.0 && theta_data[0][0] < -5.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 2, 0xf00000);
    }
  }else if(theta_data[0][0] >= -20.0 && theta_data[0][0] < -10.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 3, 0xf00000);
    }
  }else if(theta_data[0][0] >= -30.0 && theta_data[0][0] < -20.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 4, 0xf00000);
    }
  }else if(theta_data[0][0] < -30.0){
    for(int i = 0; i < 5; i++){
      M5.dis.drawpix(i * 5 + 4, 0x00f000);
    }
  }
  
  if(theta_data[0][0] > 0){
    digitalWrite(rote_pin, HIGH);
  }else{
    digitalWrite(rote_pin, LOW);
  }

  pwmDuty = max((int)map(abs(theta_data[0][0]), 50, 0, 0, 255), 0);
  ledcWrite(0, pwmDuty);

  Serial.println(theta_data[0][0]);

  delay(50);
  M5.update();
}

Increasing the inclination of M5Stack ATOM Matrix increases the rotation speed (P control).

The flywheel rotates in the direction that repels the tilt.

Currently, I am testing it by hand, but I feel a certain reaction in my hand, so

I can expect this.

Here, we made a housing for the 1-axis attitude control module.

We performed a simple operation check.

Actually I was trying and trying to make it stand upside down.

I will report to you that I was able to manage the inversion.

Physically ignore the following parameters

It took more than 5 days...

・Slope coefficient

・Tilt angular velocity coefficient

・Motor angular velocity coefficient

・Tilt offset correction

・Motor initial speed

・Brake range

Connect the PWM input line, encoder output, and brake terminals to the ATOM Matrix for control.

Motor torque in 3-axis attitude control module article Tm is controlled based on the following formula.

θb is the tilt of the module，θw is the angular velocity of the wheel，Kdx is each coefficient.

Motor current characteristics

Motor torque is proportional to the current flowing through the motor.

Since the motor of SHISEIGYO-1 controls the rotation speed by PWM input, the relationship between the OFF duty of PWM and the current flowing through the motor was measured.

When the off duty of the PWM signal to be input to the motor is raised, the rotation speed of the brushless motor ID - 549 XW rises, and the negative charge current also increases.

From the graph above, we found that duty and motor load current I were approximately linearly proportional.

The attitude angle θb of the module, the angular velocity. θb and the rotational speed of the motor duty ratio of the signal to be input to the motor can be derived from the rotation speed θw of the motor.

Moreover, it was not possible to confirm that seniigyo-1 stood only by this theoretical equation, and the following parameters were added, and each parameter was adjusted manually to aim at the collapse.

Inclination offset correction

The correction of the physical bias and the offset of the sensor is numerically corrected.

Motor speed

Offset value for canceling resistance by motor electrical resistance or physical friction

Brake range

Specifies the range of duty values to hang the brakes in order to stop the motor tightly at the standing angle.

#include "M5Atom.h"
#include <Kalman.h>

#define ENC_A 22
#define ENC_B 19
#define brake 23
#define rote_pin 32
#define PWM_pin 26
#define button 39

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

volatile byte pos;
volatile int  enc_count = 0;

int DutyIni = 490, pwmDuty;
float Kp = 4.2;
float Kd = -0.19;
float Kw = 0.1;
float D0 = 0.31;
int brakeRange = 0;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;

float theta_acc = 0.0;
float theta_dot = 0.0;

//Offset
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;

Kalman kalmanY;
float kalAngleY, kalAngleDotY;

//Sensor offset calculation
void offset_cal(){
  delay(1000);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;

  for(int i=0; i<10; i++) {
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(10);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  if(accXoffset < 0){
    accXoffset = accXoffset / 10 + 1.0 / sqrt(2.0);
  }else{
    accXoffset = accXoffset / 10 - 1.0 / sqrt(2.0);
  }
  accYoffset /= 10;
  accZoffset = accZoffset / 10 + 1.0 / sqrt(2.0);
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}

//Tilt data acquisition from acceleration sensors[deg]
float get_theta_acc() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //Tilt angle derivation unit(deg)
  theta_acc  = atan(-1.0 * (accX - accXoffset) / (accZ - accZoffset)) * 57.29578f;
  return theta_acc;
}

//Y-axis angular velocity acquisition
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_dot = gyroY - gyroYoffset;
  return theta_dot;
}

void setup() {
  M5.begin(true, false, false); //SerialEnable ,I2CEnable , LEDEnable

  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);
  pinMode(brake, OUTPUT);
  pinMode(button, INPUT);
  
  digitalWrite(brake, HIGH);
 
  attachInterrupt(ENC_A, ENC_READ, CHANGE);
  attachInterrupt(ENC_B, ENC_READ, CHANGE);

  M5.IMU.Init();

  //Sensor offset calculation
  offset_cal();

  //Full scale range
  M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS);
  M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);

  kalmanY.setAngle(get_theta_acc());

  ledcSetup(0, 20000, 9);
  ledcAttachPin(PWM_pin, 0);
  
  pinMode(rote_pin, OUTPUT);
}


void loop() {
  //Offset recomputation
  if (digitalRead(button) == 0){
    Serial.println("Offset recomputation");
    offset_cal();
  }
  
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;

  dt = (float)loopTime / 1000000.0; //sec

  //Calculation of angular velocity of motor
  float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt;
  enc_count = 0;

  //Kalman filter attitude gradient
  kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);

  //Kalman filter attitude angular velocity
  kalAngleDotY = kalmanY.getRate();

  /*
  Serial.print("kalAngleY: ");
  Serial.print(kalAngleY);
  Serial.print(", kalAngleDotY: ");
  Serial.print(kalAngleDotY);
  Serial.print(", theta_dotWheel: ");
  Serial.print(theta_dotWheel);
  */

  //Brake
  if(abs(kalAngleY) > 20.0){
    digitalWrite(brake, LOW);
  }else{
    digitalWrite(brake, HIGH);
  }

  //Serial input
  char key;

  if ( Serial.available() ) {       
    key = Serial.read();  

    switch (key) {
      case 'q':
        Kp += 0.1;
        break;
      case 'a':
        Kp -= 0.1;
        break;
      case 'w':
        Kd += 0.01;
        break;
      case 's':
        Kd -= 0.01;
        break;
      case 'e':
        Kw += 0.01;
        break;
      case 'd':
        Kw -= 0.01;
        break;
      case 'r':
        D0 += 0.01;
        break;
      case 'f':
        D0 -= 0.01;
        break;
      case 't':
        DutyIni++;
        break;
      case 'g':
        DutyIni--;
        break;
      case 'y':
        brakeRange++;
        break;
      case 'h':
        brakeRange--;
        break;
    }
  }

  Serial.print("Kp: ");
  Serial.print(Kp);
  Serial.print(", Kd: ");
  Serial.print(Kd,3);
  Serial.print(", Kw: ");
  Serial.print(Kw, 3);
  Serial.print(", D0: ");
  Serial.print(D0, 3);
  Serial.print(", DutyIni: ");
  Serial.print(DutyIni);
  Serial.print(", brakeRange: ");
  Serial.print(brakeRange);
  Serial.print(", kalAngleY: ");
  Serial.print(kalAngleY);
  
  //Motor rotation
  float M = Kp * (kalAngleY + D0) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0;
  M = max(-1.0f, min(1.0f, M));
  pwmDuty = DutyIni * (1.0 - fabs(M));
  /*
  Serial.print(", M: ");
  Serial.print(M);
  Serial.print(", pwmDuty: ");
  Serial.print(pwmDuty);
  */

  
  //Rotational direction
  if(pwmDuty > (DutyIni - brakeRange)){
    digitalWrite(brake, LOW);
    ledcWrite(0, 511);
  }else if(M > 0.0f){
    digitalWrite(rote_pin, LOW);
    ledcWrite(0, pwmDuty);
  }else{
    digitalWrite(rote_pin, HIGH);
    ledcWrite(0, pwmDuty);
  }
 
  /*
  if(M > 0){
    digitalWrite(rote_pin, LOW);
    ledcWrite(0, pwmDuty);
  }else{
    digitalWrite(rote_pin, HIGH);
    ledcWrite(0, pwmDuty);
  }
  */

  
  
  /*
  Serial.print("theta: ");
  Serial.print(kalAngleY);
  Serial.print(", PWM: ");
  Serial.print(pwmDuty);
  Serial.print(", loopTime: ");
  Serial.print((float)loopTime / 1000.0);
  Serial.print("msec, theta_dotWheel: ");
  Serial.println(theta_dotWheel);
  */


  delay(20);
  Serial.println("");
  //digitalWrite(brake, LOW);
}

void ENC_READ() {
  byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);
  byte old = pos & B00000011;
  byte dir = (pos & B00110000) >> 4;
 
  if (cur == 3) cur = 2;
  else if (cur == 2) cur = 3;
 
  if (cur != old)
  {
    if (dir == 0)
    {
      if (cur == 1 || cur == 3) dir = cur;
    } else {
      if (cur == 0)
      {
        if (dir == 1 && old == 3) enc_count--;
        else if (dir == 3 && old == 1) enc_count++;
        dir = 0;
      }
    }
 
    bool rote = 0;
    if (cur == 3 && old == 0) rote = 0;
    else if (cur == 0 && old == 3) rote = 1;
    else if (cur > old) rote = 1;
 
    pos = (dir << 4) + (old << 2) + cur;
  }
}

L. 135 ~ 139

The attitude angle (kalAngleY) and its angular velocity (kalAngleDotY) of the module are calculated using the Kalman filter library from the accelerometer and gyro with the 6-axis inertial sensor MPU6886 with built-in M5 ATOM Matrix.

L. 132

The motor rotation speed (theta_dotWheel) is calculated by counting the number of encoder steps by interruption.

L. 218 ~ 221

Calculate the duty of the PWM signal input to the motor.

The PWM resolution is 9 bits (0 to 511) (L. 111).

In addition to the attitude angle and angular velocity of the module, and the coefficients (Kp, Kd, ​​Kw) related to the motor rotation speed,

The tilt offset correction value (D0) and motor initial speed (DutyIni) are also included in the calculation.

L. 230-240

The direction of motor rotation is determined based on whether the calculated duty value is positive or negative.

In addition, a brake range is provided so that the motor stops near a balanced attitude angle.

L. 157-101

I connected a USB cable to the M5 ATOM Matrix and manually adjusted each parameter via serial input.