Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
SHISEIGYO2_02
#include <M5Core2.h>
#include <Preferences.h>
#include <Fonts/EVA_20px.h>
#include <stdio.h>
#include <Kalman.h> // https://github.com/TKJElectronics/KalmanFilter
#define ENCx_A 35
#define ENCx_B 36
#define brakeX 0
#define rote_pinX 2
#define PWM_pinX 26
#define ENCy_A 33
#define ENCy_B 32
#define brakeY 13
#define rote_pinY 14
#define PWM_pinY 25
#define CH_X 0
#define CH_Y 1
unsigned long oldTime = 0, loopTime, nowTime;
float dt;
volatile byte posX, posY;
volatile int enc_countX = 0, enc_countY = 0;
float Kp = 10;
float Kd = 10;
float Kw = 0.7;
float IDRS = 2;
int rotMax = 370;
float getupRange = 0.1;
float injectRatio = 80.0;
int pwmDutyX, pwmDutyY;
int GetUP = 0;
int GetUpcnt = 0;
int cnt = 99;
float Mx, My;
float AjX = 0.0, AjY = 0.0;
float rot = 0.0, rotX = 0.0, rotY = 0.0;
float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float theta_X = 0.0, theta_Y = 0.0;
float theta_Xdot = 0.0, theta_Ydot = 0.0;
//オフセット
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;
Kalman kalmanX, kalmanY;
float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;
static TFT_eSprite *sprite = nullptr;
static bool IMURunning = false;
int scene = 0;
Preferences preferences;
//センサオフセット算出
void offset_cal(){
delay(300);
M5.Lcd.fillScreen(TFT_RED);
delay(300);
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(50);
accXoffset += accX;
accYoffset += accY;
accZoffset += accZ;
gyroXoffset += gyroX;
gyroYoffset += gyroY;
gyroZoffset += gyroZ;
}
accXoffset = accXoffset / 10 + 1.0 / sqrt(2.0);
accYoffset = accYoffset / 10 + 1.0 / sqrt(2.0);
accZoffset /= 10.0;
gyroXoffset /= 10.0;
gyroYoffset /= 10.0;
gyroZoffset /= 10.0;
M5.Lcd.fillScreen(TFT_BLACK);
}
//加速度センサから傾きデータ取得 [deg]
void get_theta() {
M5.IMU.getAccelData(&accX,&accY,&accZ);
//傾斜角導出 単位はdeg
theta_X = atan2(+1.0 * (accY - accYoffset) , (accZ - accZoffset)) * 180.0/PI;
theta_Y = atan2(-1.0 * (accX - accXoffset) , (accZ - accZoffset)) * 180.0/PI;
}
//角速度取得
void get_gyro_data() {
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
theta_Xdot = gyroX - gyroXoffset;
theta_Ydot = gyroY - gyroYoffset;
}
//起き上がり
void getUp() {
Serial.println("getUp!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
if(kalAngleX < -14.0 && kalAngleX > -24.0 && kalAngleY > 14.0 && kalAngleY < 24.0){
M5.Axp.SetLDOEnable(3, true);
delay(500);
M5.Axp.SetLDOEnable(3, false);
GetUP = 1;
AjX = 0.0;
AjY = 0.0;
digitalWrite(brakeX, HIGH);
digitalWrite(brakeY, HIGH);
digitalWrite(rote_pinX, HIGH);
digitalWrite(rote_pinY, LOW);
for(int i = 1023; i >= rotMax; i--){
ledcWrite(CH_X, i);
ledcWrite(CH_Y, i);
cnt = map(i, 1023, rotMax, 3, 0);
Serial.println(cnt);
delay(5);
}
ledcWrite(CH_X, rotMax);
ledcWrite(CH_Y, rotMax);
delay(300);
scene = 0;
cnt = 99;
digitalWrite(rote_pinX, LOW);
digitalWrite(rote_pinY, HIGH);
}
}
void ENCx_READ() {
byte cur = (!digitalRead(ENCx_B) << 1) + !digitalRead(ENCx_A);
byte old = posX & B00000011;
byte dir = (posX & 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_countX--;
else if (dir == 3 && old == 1) enc_countX++;
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;
posX = (dir << 4) + (old << 2) + cur;
}
}
void ENCy_READ() {
byte cur = (!digitalRead(ENCy_B) << 1) + !digitalRead(ENCy_A);
byte old = posY & B00000011;
byte dir = (posY & 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_countY--;
else if (dir == 3 && old == 1) enc_countY++;
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;
posY = (dir << 4) + (old << 2) + cur;
}
}
//ディスプレイ表示 Core0
void displayLoop(void *pvParameters) {
disableCore0WDT(); // only needs to be called once
sprite = new TFT_eSprite((M5Display*)pvParameters);
// sprite->setPsram( false ); // faster rendering without psram but requires to patch sprite.cpp
sprite->setColorDepth( 8 ); // however use 8 bits colors because there isn't enough ram for 16 bits
sprite->setFreeFont( nullptr ); // for some reason sprites aren't initiated with a font
sprite->setTextColor( TFT_WHITE );
sprite->createSprite( 320, 240 );
uint32_t colorX, colorY;
int xx, yy, lastxx =-1, lastyy =-1;
char Str[32];
while( !IMURunning ) {
// wait until the other task has fully started
vTaskDelay(1);
}
while(1) {
vTaskDelay(1);
//Main Display
if(scene == 0){
xx = map( kalAngleX, 15, -15, 0, 240 );
yy = map( kalAngleY, 20, -20, 0, 320 );
if( xx == lastxx && yy == lastyy ) {
// nothing to draw
continue;
}
sprite->fillSprite( TFT_BLACK );
if(fabs(kalAngleX) <= 1.0){
colorX = GREEN;
} else {
colorX = RED;
}
if(fabs(kalAngleY) <= 1.0){
colorY = GREEN;
} else {
colorY = BLUE;
}
sprite->fillRect( 0, xx - 2, 320, 5, colorX );
sprite->fillRect( yy - 2, 0, 5, 240, colorY );
lastxx = xx;
lastyy = yy;
sprite->drawRoundRect(0, 25, 155, 37, 5, WHITE);
sprite->drawRoundRect(0, 75, 155, 37, 5, WHITE);
sprite->drawRoundRect(0, 125, 155, 37, 5, WHITE);
sprite->drawRoundRect(165, 25, 155, 37, 5, WHITE);
sprite->drawRoundRect(165, 75, 155, 37, 5, WHITE);
sprite->drawRoundRect(165, 125, 155, 37, 5, WHITE);
sprite->drawRoundRect(165, 175, 155, 37, 5, WHITE);
sprite->setTextSize(2);
snprintf( Str, 32, "X:%.2f", kalAngleX );
sprite->drawString( Str, 10, 0 );
snprintf( Str, 32, "Y:%.2f", kalAngleY );
sprite->drawString(Str, 140, 0);
sprite->setTextSize(3);
snprintf( Str, 32, "rMax:%d", rotMax );
sprite->drawString(Str, 7, 33);
snprintf( Str, 32, "GR:%.1f", getupRange );
sprite->drawString(Str, 7, 83);
snprintf( Str, 32, "IR:%.0f", injectRatio );
sprite->drawString(Str, 7, 133);
snprintf( Str, 32, "Kp:%.1f", Kp );
sprite->drawString( Str, 170, 33 );
snprintf( Str, 32, "Kd:%.1f", Kd );
sprite->drawString( Str, 170, 83 );
snprintf( Str, 32, "Kw:%.1f", Kw );
sprite->drawString( Str, 170, 133 );
snprintf( Str, 32, "IDRS:%.1f", IDRS );
sprite->drawString( Str, 170, 183 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "GetUP" );
sprite->drawString( Str, 25, 224 );
sprite->pushSprite( 0, 0 );
}
//rotMax
if(scene == 1){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "rotMax" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Maximum rotation speed" );
sprite->drawString( Str, 20, 40 );
snprintf( Str, 32, "%s", "when getting up" );
sprite->drawString( Str, 20, 56 );
sprite->setTextSize(4);
snprintf( Str, 32, "%d", rotMax );
sprite->drawString(Str, 125, 120);
}
//getupRange
if(scene == 2){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "getupRange" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Angle to stop getting up" );
sprite->drawString( Str, 20, 40 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.1f", getupRange );
sprite->drawString(Str, 125, 120);
}
//injectRatio
if(scene == 3){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "injectRatio" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Ratio of angle and" );
sprite->drawString( Str, 20, 40 );
snprintf( Str, 32, "%s", "rotation when getting up" );
sprite->drawString( Str, 20, 56 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.0f", injectRatio );
sprite->drawString(Str, 125, 120);
}
//Kp
if(scene == 4){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "Kp" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Coefficient for module angle" );
sprite->drawString( Str, 20, 40 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.1f", Kp );
sprite->drawString(Str, 125, 120);
}
//Kd
if(scene == 5){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "Kd" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Coefficient for " );
sprite->drawString( Str, 20, 40 );
snprintf( Str, 32, "%s", "module angular velocity" );
sprite->drawString( Str, 20, 56 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.1f", Kd );
sprite->drawString(Str, 125, 120);
}
//Kw
if(scene == 6){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "Kw" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Coefficient for " );
sprite->drawString( Str, 20, 40 );
snprintf( Str, 32, "%s", "wheel rotation speed" );
sprite->drawString( Str, 20, 56 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.1f", Kw );
sprite->drawString(Str, 125, 120);
}
//IDRS
if(scene == 7){
sprite->fillSprite( TFT_BLACK );
sprite->setTextSize(3);
snprintf( Str, 32, "%s", "IDRS" );
sprite->drawString( Str, 10, 10 );
sprite->setTextSize(2);
snprintf( Str, 32, "%s", "Inverted angle Dynamic" );
sprite->drawString( Str, 20, 40 );
snprintf( Str, 32, "%s", "Readjustment System" );
sprite->drawString( Str, 20, 56 );
sprite->setTextSize(4);
snprintf( Str, 32, "%.1f", IDRS );
sprite->drawString(Str, 125, 120);
}
//GetUp
if(scene == 8){
if(cnt == 0){
sprite->fillSprite( TFT_WHITE );
}else{
sprite->fillSprite( TFT_BLACK );
}
sprite->setTextSize(7);
if(cnt == 3){
snprintf( Str, 32, "%s", "3" );
sprite->drawString(Str, 135, 95);
}
if(cnt == 2){
snprintf( Str, 32, "%s", "2" );
sprite->drawString(Str, 135, 95);
}
if(cnt == 1){
snprintf( Str, 32, "%s", "1" );
sprite->drawString(Str, 135, 95);
}
sprite->pushSprite( 0, 0 );
}
if(scene != 0 && scene != 8){
sprite->setTextSize(2);
snprintf( Str, 32, "%s", " - " );
sprite->drawString( Str, 35, 224 );
snprintf( Str, 32, "%s"," + " );
sprite->drawString( Str, 142, 224 );
snprintf( Str, 32, "%s", "Back" );
sprite->drawString( Str, 243, 224 );
sprite->pushSprite( 0, 0 );
}
}
sprite->deleteSprite();
vTaskDelete( NULL );
}
void IMULoop( void * param ) {
M5.IMU.Init();
//フルスケールレンジ
M5.IMU.SetAccelFsr(M5.IMU.AFS_2G);
M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS);
pinMode(rote_pinX, OUTPUT);
pinMode(brakeX, OUTPUT);
digitalWrite(brakeX, LOW);
pinMode(rote_pinY, OUTPUT);
pinMode(brakeY, OUTPUT);
digitalWrite(brakeY, LOW);
pinMode(ENCx_A, INPUT);
pinMode(ENCx_B, INPUT);
pinMode(ENCy_A, INPUT);
pinMode(ENCy_B, INPUT);
attachInterrupt(ENCx_A, ENCx_READ, CHANGE);
attachInterrupt(ENCx_B, ENCx_READ, CHANGE);
attachInterrupt(ENCy_A, ENCy_READ, CHANGE);
attachInterrupt(ENCy_B, ENCy_READ, CHANGE);
ledcSetup(CH_X, 20000, 10);
ledcAttachPin(PWM_pinX, CH_X);
ledcSetup(CH_Y, 20000, 10);
ledcAttachPin(PWM_pinY, CH_Y);
delay(500);
//センサオフセット算出
offset_cal();
get_theta();
kalmanX.setAngle(theta_X);
kalmanY.setAngle(theta_Y);
IMURunning = true;
while(1) {
nowTime = micros();
loopTime = nowTime - oldTime;
oldTime = nowTime;
dt = (float)loopTime / 1000000.0; //sec
//モータの角速度算出
float theta_XdotWheel = -1.0 * float(enc_countX) * 3.6 / dt;
enc_countX = 0;
float theta_YdotWheel = -1.0 * float(enc_countY) * 3.6 / dt;
enc_countY = 0;
get_theta();
get_gyro_data();
//カルマンフィルタ 姿勢 傾き
kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt);
kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt);
//カルマンフィルタ 姿勢 角速度
kalAngleDotX = kalmanX.getRate();
kalAngleDotY = kalmanY.getRate();
//タッチボタン
TouchPoint_t pos = M5.Touch.getPressPoint();
if(pos.y > 240 && pos.x < 106 && GetUP != 99) {
switch(scene){
case 0: //Main
scene = 8;
getUp();
break;
case 1: //rotMax
if(rotMax > 0) rotMax -= 10;
delay(200);
break;
case 2: //getupRange
if(getupRange > -1.0) getupRange -= 0.1;
delay(200);
break;
case 3: //injectRatio
if(injectRatio > 0) injectRatio -= 2;
delay(200);
break;
case 4: //Kp
if(Kp > 0) Kp -= 0.1;
delay(200);
break;
case 5: //Kd
if(Kd > 0) Kd -= 0.1;
delay(200);
break;
case 6: //Kw
if(Kw > 0) Kw -= 0.1;
delay(200);
break;
case 7: //IDRS
if(IDRS > 0) IDRS -= 0.1;
delay(200);
break;
}
}else if(pos.y > 240 && pos.x < 213){
switch(scene){
case 1: //rotMax
if(rotMax < 900) rotMax += 10;
delay(200);
break;
case 2: //getupRange
if(getupRange < 1.0) getupRange += 0.1;
delay(200);
break;
case 3: //injectRatio
if(injectRatio < 200) injectRatio += 2;
delay(200);
break;
case 4: //Kp
if(Kp < 20) Kp += 0.1;
delay(200);
break;
case 5: //Kd
if(Kd < 20) Kd += 0.1;
delay(200);
break;
case 6: //Kw
if(Kw < 20) Kw += 0.1;
delay(200);
break;
case 7: //IDRS
if(IDRS < 10) IDRS += 0.1;
delay(200);
break;
}
}else if(pos.y > 240 && pos.x < 320){
if(scene != 0){
switch(scene){ //パラメータ保存
case 1: //rotMax
preferences.putInt("rotMax", rotMax);
break;
case 2: //getupRange
preferences.putFloat("getupRange", getupRange);
break;
case 3: //injectRatio
preferences.putFloat("injectRatio", injectRatio);
break;
case 4: //Kp
preferences.putFloat("Kp", Kp);
break;
case 5: //Kd
preferences.putFloat("Kd", Kd);
break;
case 6: //Kw
preferences.putFloat("Kw", Kw);
break;
case 7: //IDRS
preferences.putFloat("IDRS", IDRS);
break;
}
scene = 0;
M5.Axp.SetLDOEnable(3, true);
delay(300);
M5.Axp.SetLDOEnable(3, false);
}
}
//パラメータボタン
if(GetUP != 99){
//rotMax
if(pos.x < 165 && pos.y > 25 && pos.y < 75 && scene == 0) {
scene = 1;
}
//getupRange
if(pos.x < 165 && pos.y > 75 && pos.y < 125 && scene == 0) {
scene = 2;
}
//injectRatio
if(pos.x < 165 && pos.y > 125 && pos.y < 175 && scene == 0) {
scene = 3;
}
//Kp
if(pos.x > 165 && pos.y > 25 && pos.y < 75 && scene == 0) {
scene = 4;
}
//Kd
if(pos.x > 165 && pos.y > 75 && pos.y < 125 && scene == 0) {
scene = 5;
}
//Kw
if(pos.x > 165 && pos.y > 125 && pos.y < 175 && scene == 0) {
scene = 6;
}
//IDRS
if(pos.x > 165 && pos.y > 175 && pos.y < 225 && scene == 0) {
scene = 7;
}
}
//起き上がり動作
if(GetUP == 1){
if(kalAngleX >= getupRange && kalAngleY <= -getupRange){
digitalWrite(brakeX, LOW);
digitalWrite(brakeY, LOW);
GetUP = 99;
}else{
if(kalAngleX < 0){
ledcWrite(CH_X, max(1023 - int(injectRatio * fabs(kalAngleX)), 0));
}
if(kalAngleY > 0){
ledcWrite(CH_Y, max(1023 - int(injectRatio * fabs(kalAngleY)), 0));
}
}
}
if ((fabs(kalAngleX) < 1 && fabs(kalAngleY) < 1) && GetUP == 0 ){
GetUP = 99;
}
if(GetUP == 99){
//ブレーキ
if(fabs(kalAngleX) > 14.0 || fabs(kalAngleY) > 14.0){
digitalWrite(brakeY, LOW);
digitalWrite(brakeX, LOW);
GetUP = 0;
AjX = 0.0;
AjY = 0.0;
}else{
digitalWrite(brakeX, HIGH);
digitalWrite(brakeY, HIGH);
}
//モータ回転
AjX += IDRS * theta_XdotWheel / 1000000.0;
AjY += IDRS * theta_YdotWheel / 1000000.0;
Mx = Kp * (kalAngleX + AjX) / 90.0 + Kd * kalAngleDotX / 500.0 + Kw * theta_XdotWheel / 10000.0;
My = Kp * (kalAngleY + AjY) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_YdotWheel / 10000.0;
Mx = max(-1.0f, min(1.0f, Mx));
pwmDutyX = 1023 * (1.0 - fabs(Mx));
My = max(-1.0f, min(1.0f, My));
pwmDutyY = 1023 * (1.0 - fabs(My));
//回転方向
if(pwmDutyX > 1000){
digitalWrite(brakeX, LOW);
ledcWrite(CH_X, 1023);
}else if(Mx > 0.0){
digitalWrite(rote_pinX, HIGH);
ledcWrite(CH_X, pwmDutyX);
}else{
digitalWrite(rote_pinX, LOW);
ledcWrite(CH_X, pwmDutyX);
}
if(pwmDutyY > 1000){
digitalWrite(brakeY, LOW);
ledcWrite(CH_Y, 1023);
}else if(My < 0.0){
digitalWrite(rote_pinY, LOW);
ledcWrite(CH_Y, pwmDutyY);
}else{
digitalWrite(rote_pinY, HIGH);
ledcWrite(CH_Y, pwmDutyY);
}
}
Serial.print("loopTime: ");
Serial.print((float)loopTime / 1000.0);
Serial.println("");
//vTaskDelay(1);
if(GetUP == 99) {
delay(6);
}else{
delay(1);
}
}
vTaskDelete( NULL );
}
void setup() {
M5.begin(true, true, true, true); //LCD, SD, Serial, I2C
M5.Lcd.fillScreen(TFT_BLACK);
preferences.begin("parameter", false);
//パラメータ初期値取得
rotMax = preferences.getInt("rotMax", rotMax);
getupRange = preferences.getFloat("getupRange", getupRange);
injectRatio = preferences.getFloat("injectRatio", injectRatio);
Kp = preferences.getFloat("Kp", Kp);
Kd = preferences.getFloat("Kd", Kd);
Kw = preferences.getFloat("Kw", Kw);
IDRS = preferences.getFloat("IDRS", IDRS);
enum CPUCores {
ESP32_CORE0 = 0,
ESP32_CORE1 = 1
};
//ディスプレイ表示 タスク
//xTaskCreatePinnedToCore( task, "task", stacksize, &object, (int)priority, taskhandle, (int)core );
xTaskCreatePinnedToCore( IMULoop, "IMULoop", 4096, NULL, 2, NULL, ESP32_CORE1 ); // I2C prefers core 1
xTaskCreatePinnedToCore( displayLoop, "displayLoop", 4096, &M5.Lcd, 1, NULL, ESP32_CORE0 ); // LCD will run on core 0
}
void loop() {
// don't put anything else in there
vTaskDelete( NULL );
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment