├── GyroM5 └── GyroM5.ino ├── GyroM5Atom ├── GyroM5Atom.hpp └── GyroM5Atom.ino ├── GyroM5Stick └── GyroM5Stick.ino ├── GyroM5StickPlus └── GyroM5StickPlus.ino ├── LICENSE ├── README-en.md ├── README.md ├── _config.yml └── google6cdfcb07c2c314de.html /GyroM5/GyroM5.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////// 2 | // M5StickC Project: 3 | // Steering Assisting Unit for RC Car 4 | // PID Controler: 5 | // error = ch1_in - Kg*yaw_rate 6 | // ch1_out = Kp*( error + Ki*LPF(error) + Kd*HPF(error) ) 7 | // URL: 8 | // https://github.com/hshin-git/GyroM5 9 | ////////////////////////////////////////////////// 10 | #include 11 | 12 | ////////////////////////////////////////////////// 13 | // Global constants 14 | ////////////////////////////////////////////////// 15 | // PWM parameters 16 | const int PWM_HERZ = 50; 17 | const int PWM_BITS = 16; 18 | const int PWM_CH1 = 0; 19 | const int PWM_CH2 = 1; 20 | // 21 | const int PWM_DMAX = (1< 146 | Preferences PARAM; 147 | // CH1 parameters 148 | const int DMIN = 0; // CH1 minimum 149 | const int DMAX = 1; // CH1 maximum 150 | const int ZERO = 2; // CH1 neutral (not in use) 151 | // PID parameters 152 | const int I_KG = 3; // PID gain KG 153 | const int I_KP = 4; // PID gain KP 154 | const int I_KI = 5; // PID gain KI 155 | const int I_KD = 6; // PID gain KD 156 | // 157 | int CONF[8]; 158 | // conf print 159 | void conf_show() { 160 | lcd_header("\nCONF\n"); 161 | // duty range 162 | M5.Lcd.printf(" DMIN: %6d\n",CONF[DMIN]); 163 | M5.Lcd.printf(" DMAX: %6d\n",CONF[DMAX]); 164 | //M5.Lcd.printf(" ZERO: %6d\n",CONF[ZERO]); 165 | // gain param 166 | M5.Lcd.printf(" KG: %6d\n",CONF[I_KG]); 167 | M5.Lcd.printf(" KP: %6d\n",CONF[I_KP]); 168 | M5.Lcd.printf(" KI: %6d\n",CONF[I_KI]); 169 | M5.Lcd.printf(" KD: %6d\n",CONF[I_KD]); 170 | } 171 | // conf ch1 duty range 172 | void conf_range() { 173 | int ch1,val; 174 | for (int n=0; n<2; n++) { 175 | delay(500); 176 | while (true) { 177 | ch1 = pulseIn(CH1_IN,HIGH,PWM_WAIT); 178 | val = map(ch1, 0,PWM_CYCL, 0,PWM_DMAX); 179 | ledcWrite(PWM_CH1,(ch1>0? val: 0)); 180 | lcd_clear(); 181 | M5.Lcd.printf("\n%s\n", (n? "LEFT": "RIGHT")); 182 | M5.Lcd.printf("[A] SAVE\n"); 183 | M5.Lcd.printf("[B] CANCEL\n"); 184 | M5.Lcd.printf(" CH1: %6d\n",ch1); 185 | M5.Lcd.printf(" VAL: %6d\n",val); 186 | delay(50); 187 | M5.update(); 188 | if (M5.BtnA.isPressed()) { 189 | if (ch1>0) CONF[(n? DMAX: DMIN)] = val; 190 | break; 191 | } 192 | if (M5.BtnB.isPressed()) { 193 | return; 194 | } 195 | } 196 | } 197 | // save 198 | if (ch1>0) { 199 | if (CONF[DMAX] < CONF[DMIN]) { 200 | int temp = CONF[DMAX]; 201 | CONF[DMAX] = CONF[DMIN]; 202 | CONF[DMIN] = temp; 203 | } 204 | PARAM.putBytes("CONF",&CONF,sizeof(CONF)); 205 | } 206 | conf_show(); 207 | delay(5*1000); 208 | } 209 | // conf gyro gain 210 | void conf_value(int pin, int pos, char *str) { 211 | int ch1,val; 212 | int old = CONF[pos]; 213 | delay(500); 214 | // Gain 215 | while (true) { 216 | ch1 = pulseIn(pin,HIGH,PWM_WAIT); 217 | val = map(ch1, PULSE_MIN,PULSE_MAX, -RANGE_MAX,RANGE_MAX); 218 | lcd_header(str); 219 | M5.Lcd.printf("[A] SAVE\n"); 220 | M5.Lcd.printf("[B] CANCEL\n"); 221 | M5.Lcd.printf(" CH1: %6d\n",ch1); 222 | M5.Lcd.printf(" NEW: %6d\n",val); 223 | M5.Lcd.printf(" OLD: %6d\n",old); 224 | delay(50); 225 | M5.update(); 226 | if (M5.BtnA.isPressed()) { 227 | if (ch1>0) CONF[pos] = val; 228 | break; 229 | } 230 | if (M5.BtnB.isPressed()) { 231 | return; 232 | } 233 | } 234 | // save 235 | if (ch1>0) { 236 | PARAM.putBytes("CONF",&CONF,sizeof(CONF)); 237 | } 238 | conf_show(); 239 | delay(5*1000); 240 | } 241 | // 242 | void conf_menu() { 243 | unsigned long enter = millis(); 244 | int pos = 0; 245 | delay(500); 246 | while (millis() < enter + 5*1000) { 247 | lcd_clear(); 248 | M5.Lcd.printf("\nCONF MENU\n"); 249 | M5.Lcd.printf("[A] SELECT\n"); 250 | M5.Lcd.printf("[B] NEXT\n"); 251 | M5.Lcd.printf(" %s PID KG\n", (pos==0? ">": " ")); 252 | M5.Lcd.printf(" %s PID KP\n", (pos==1? ">": " ")); 253 | M5.Lcd.printf(" %s PID KI\n", (pos==2? ">": " ")); 254 | M5.Lcd.printf(" %s PID KD\n", (pos==3? ">": " ")); 255 | M5.Lcd.printf(" %s CH1 ZERO\n",(pos==4? ">": " ")); 256 | M5.Lcd.printf(" %s CH1 DUTY\n",(pos==5? ">": " ")); 257 | delay(200); 258 | M5.update(); 259 | if (M5.BtnA.isPressed()) { 260 | switch (pos) { 261 | case 0: conf_value(CH1_IN,I_KG,"\nPID KG\n"); break; 262 | case 1: conf_value(CH1_IN,I_KP,"\nPID KP\n"); break; 263 | case 2: conf_value(CH1_IN,I_KI,"\nPID KI\n"); break; 264 | case 3: conf_value(CH1_IN,I_KD,"\nPID KD\n"); break; 265 | case 4: zero_calibration(); break; 266 | case 5: conf_range(); break; 267 | default: break; 268 | } 269 | } 270 | if (M5.BtnB.isPressed()) { 271 | enter = millis(); 272 | pos = (pos + 1) % 6; 273 | } 274 | //delay(200); 275 | } 276 | } 277 | 278 | 279 | ////////////////////////////////////////////////// 280 | // CH3 mode switcher 281 | ////////////////////////////////////////////////// 282 | // CH3 PWM width and mode 283 | int CH3_USEC = 0; 284 | int CH3_MODE = 0; 285 | // 286 | void ch3_select() { 287 | unsigned long enter = millis(); 288 | delay(500); 289 | while (millis() < enter + 5*1000) { 290 | lcd_clear(); 291 | M5.Lcd.printf("\nCH3 MODE\n"); 292 | M5.Lcd.printf("[B] NEXT\n"); 293 | if (CH3_USEC > 0) { 294 | M5.Lcd.printf(" %s PID TAB\n", (CH3_MODE==0? ">": " ")); 295 | M5.Lcd.printf(" %s PID KG\n", (CH3_MODE==1? ">": " ")); 296 | M5.Lcd.printf(" %s PID KP\n", (CH3_MODE==2? ">": " ")); 297 | M5.Lcd.printf(" %s PID KI\n", (CH3_MODE==3? ">": " ")); 298 | M5.Lcd.printf(" %s PID KD\n", (CH3_MODE==4? ">": " ")); 299 | M5.Lcd.printf(" %s PID OFF\n", (CH3_MODE==5? ">": " ")); 300 | } else { 301 | M5.Lcd.printf(" %s PID TAB\n", (CH3_MODE==0? ">": " ")); 302 | M5.Lcd.printf(" %s PID OFF\n", (CH3_MODE==1? ">": " ")); 303 | } 304 | delay(200); 305 | M5.update(); 306 | if (M5.BtnB.isPressed()) { 307 | enter = millis(); 308 | if (CH3_USEC>0) 309 | CH3_MODE = (CH3_MODE + 1) % 6; 310 | else 311 | CH3_MODE = (CH3_MODE + 1) % 2; 312 | } 313 | //delay(200); 314 | } 315 | } 316 | 317 | 318 | ////////////////////////////////////////////////// 319 | // put your setup code here, to run once: 320 | ////////////////////////////////////////////////// 321 | void setup() { 322 | // (1) Initialize the M5StickC object 323 | M5.begin(); 324 | M5.MPU6886.Init(); 325 | M5.Axp.ScreenBreath(10); 326 | //while (!setCpuFrequencyMhz(80)); 327 | 328 | // (2) Initialize GPIO 329 | pinMode(CH1_IN,INPUT); 330 | pinMode(CH3_IN,INPUT); 331 | 332 | pinMode(CH1_OUT,OUTPUT); 333 | ledcSetup(PWM_CH1,PWM_HERZ,PWM_BITS); 334 | ledcAttachPin(CH1_OUT,PWM_CH1); 335 | 336 | // (3) Initialize parameters 337 | PARAM.begin("GYRO"); 338 | PARAM.getBytes("CONF",&CONF,sizeof(CONF)); 339 | 340 | // (4) Initialize filters 341 | lpf_init(OMEGA_LPF,2.0/100); 342 | lpf_init(CH1DT_LPF,2.0/100); 343 | lpf_init(CH1ER_LPF,2.0/ 50); 344 | hpf_init(CH1ER_HPF,50./100); 345 | 346 | // (5) Calibrate zero points 347 | zero_calibration(); 348 | 349 | // (6) Do others 350 | //Serial.begin(115200); 351 | } 352 | 353 | 354 | ////////////////////////////////////////////////// 355 | // put your main code here, to run repeatedly: 356 | ////////////////////////////////////////////////// 357 | // PWM widths in usec 358 | int CH1_USEC = 0; 359 | int CH2_USEC = 0; 360 | // IMU values 361 | float OMEGA[3]; 362 | float ACCEL[3]; 363 | // Main loop counter 364 | int LOOP = 0; 365 | 366 | void loop() { 367 | int ch1_duty,ch1_dout; 368 | int ch3_gain,KG,KP,KI,KD; 369 | float omega,error; 370 | bool drift = false; 371 | 372 | // (1) Input PWM values 373 | CH1_USEC = pulseIn(CH1_IN,HIGH,PWM_WAIT); 374 | if (LOOP%25 == 0) CH3_USEC = pulseIn(CH3_IN,HIGH,PWM_WAIT); 375 | 376 | // (2) Input IMU values 377 | M5.MPU6886.getGyroData(&OMEGA[0],&OMEGA[1],&OMEGA[2]); 378 | //if (LOOP%25 == 0) M5.MPU6886.getAccelData(&ACCEL[0],&ACCEL[1],&ACCEL[2]); 379 | 380 | // (3) Compute PWM value by PID 381 | ch1_duty = map(CH1_USEC, 0,PWM_CYCL, 0,PWM_DMAX); 382 | ch3_gain = map(CH3_USEC, PULSE_MIN,PULSE_MAX, -RANGE_MAX,RANGE_MAX); 383 | 384 | // CH3 mode -> See ch3_select(). 385 | KG = CONF[I_KG]; 386 | KP = CONF[I_KP]; 387 | KI = CONF[I_KI]; 388 | KD = CONF[I_KD]; 389 | if (CH3_USEC > 0) { 390 | switch (CH3_MODE) { 391 | case 1: KG = ch3_gain; break; 392 | case 2: KP = ch3_gain; break; 393 | case 3: KI = ch3_gain; break; 394 | case 4: KD = ch3_gain; break; 395 | case 5: KP = 50; KG = KI = KD = 0; break; 396 | default: break; 397 | } 398 | } else { 399 | switch (CH3_MODE) { 400 | case 1: KP = 50; KG = KI = KD = 0; break; 401 | default: break; 402 | } 403 | } 404 | 405 | // CH1 duty out 406 | omega = (OMEGA[2] * M5.MPU6886.gRes) - OMEGA_ZERO; 407 | error = (ch1_duty - CH1DT_ZERO) - (KG/0.5)*omega; 408 | ch1_dout = int(CH1DT_ZERO + (KP/50.0)*(error + (KI/50.0)*lpf_update(CH1ER_LPF,error) + (KD/50.0)*hpf_update(CH1ER_HPF,error))); 409 | ch1_dout = constrain(ch1_dout, CONF[DMIN],CONF[DMAX]); 410 | ch1_dout = (CH1_USEC>0? ch1_dout: 0); 411 | 412 | // Drifting? 413 | drift = omega * (ch1_dout - CH1DT_ZERO) < -500. ? true: false; 414 | 415 | // (4) Output PWM value 416 | ledcWrite(PWM_CH1,ch1_dout); 417 | 418 | // (5) Update LCD 419 | if (LOOP%10 == 0) { 420 | lcd_clear(drift? TFT_PINK: TFT_WHITE); 421 | M5.Lcd.printf("\nINPUT (us)\n"); 422 | M5.Lcd.printf(" CH1:%6d\n",CH1_USEC); 423 | //M5.Lcd.printf(" CH2:%6d\n",CH2_USEC); 424 | M5.Lcd.printf(" CH3:%6d\n",CH3_USEC); 425 | M5.Lcd.printf("\nOMEGA (rad/s)\n"); 426 | M5.Lcd.printf(" X:%7.2f\n",OMEGA[0] *M5.MPU6886.gRes); 427 | M5.Lcd.printf(" Y:%7.2f\n",OMEGA[1] *M5.MPU6886.gRes); 428 | M5.Lcd.printf(" Z:%7.2f\n",OMEGA[2] *M5.MPU6886.gRes); 429 | //M5.Lcd.printf("\nACCEL (G)\n"); 430 | //M5.Lcd.printf(" X:%7.2f\n",ACCEL[0]); 431 | //M5.Lcd.printf(" Y:%7.2f\n",ACCEL[1]); 432 | //M5.Lcd.printf(" Z:%7.2f\n",ACCEL[2]); 433 | M5.Lcd.printf("\nDUTY (16bit)\n"); 434 | M5.Lcd.printf(" I:%6d\n",ch1_duty); 435 | M5.Lcd.printf(" O:%6d\n",ch1_dout); 436 | M5.Lcd.printf(" E:%6d\n",int(error)); 437 | M5.Lcd.printf("\nGAIN (0-100)\n"); 438 | M5.Lcd.printf(" G/P:%3d/%3d\n",KG,KP); 439 | M5.Lcd.printf(" I/D:%3d/%3d\n",KI,KD); 440 | // watch vin and buttons 441 | vin_watch(); 442 | M5.update(); 443 | if (M5.BtnA.isPressed()) conf_menu(); 444 | if (M5.BtnB.isPressed()) ch3_select(); 445 | } 446 | 447 | // (6) Do others 448 | LOOP = (LOOP + 1) % 50; 449 | } 450 | -------------------------------------------------------------------------------- /GyroM5Atom/GyroM5Atom.hpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // ドリフトRCカー用ジャイロシステムGyroM5Atom 3 | // GyroM5Atom system for RC drift car 4 | // https://github.com/hshin-git/GyroM5 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define DEBUG Serial 14 | 15 | 16 | //////////////////////////////////////////////////////////////////////////////// 17 | // class CONFIG{}: 設定パラメータの管理クラス 18 | // init(): パラメータの初期値 19 | // load(): パラメータの復元 20 | // save(): パラメータの保存 21 | // setup(): パラメータの初期化 22 | // getJSON(): パラメータのJSON文字列 23 | // setCONF(): パラメータの更新 24 | //////////////////////////////////////////////////////////////////////////////// 25 | Preferences CONFIG_PREF; 26 | 27 | class CONFIG { 28 | // 29 | static const char JSON[]; 30 | #define CONFIG_NAME "GyroM5Atom" 31 | #define CONFIG_KEY "GyroM5Atom" 32 | #define CONFIG_MAGIC 12345 33 | // 34 | public: 35 | // setting parameters 36 | int MODE; 37 | int KG; 38 | int KP; 39 | int KI; 40 | int KD; 41 | int REV; 42 | int MIN; 43 | int MAX; 44 | int MEAN; 45 | int ROLL; 46 | int FREQ; 47 | int AXIS; 48 | int MAGIC; 49 | // 50 | void init() { 51 | MODE = 0; 52 | KG = 50; 53 | KP = 50; 54 | KI = 10; 55 | KD = 5; 56 | REV = 1; 57 | MIN = 1000; 58 | MAX = 2000; 59 | MEAN = 1500; 60 | ROLL = 45; 61 | FREQ = 50; 62 | AXIS = 1; 63 | MAGIC = CONFIG_MAGIC; 64 | } 65 | void load() { 66 | if (CONFIG_PREF.begin(CONFIG_NAME)) { 67 | CONFIG_PREF.getBytes(CONFIG_KEY,(uint8_t*)this,sizeof(*this)); 68 | CONFIG_PREF.end(); 69 | } 70 | } 71 | void save() { 72 | if (CONFIG_PREF.begin(CONFIG_NAME)) { 73 | CONFIG_PREF.putBytes(CONFIG_KEY,(uint8_t*)this,sizeof(*this)); 74 | CONFIG_PREF.end(); 75 | } 76 | } 77 | void setup() { 78 | load(); 79 | if (MAGIC != CONFIG_MAGIC) { 80 | init(); save(); 81 | } 82 | } 83 | char *getJSON() { 84 | static char json[1024]; 85 | sprintf(json, JSON, MODE,KG,KP,KI,KD,REV,MIN,MAX,MEAN,ROLL,FREQ,AXIS); 86 | DEBUG.println(json); 87 | return json; 88 | } 89 | void setCONF(const char *key, int val) { 90 | DEBUG.print(key); DEBUG.print("="); DEBUG.println(val); 91 | if (strcmp(key,"MODE")==0) MODE = val; 92 | else if (strcmp(key,"KG")==0) KG = val; 93 | else if (strcmp(key,"KP")==0) KP = val; 94 | else if (strcmp(key,"KI")==0) KI = val; 95 | else if (strcmp(key,"KD")==0) KD = val; 96 | else if (strcmp(key,"REV")==0) REV = val; 97 | else if (strcmp(key,"MIN")==0) MIN = val; 98 | else if (strcmp(key,"MAX")==0) MAX = val; 99 | else if (strcmp(key,"MEAN")==0) MEAN = val; 100 | else if (strcmp(key,"ROLL")==0) ROLL = val; 101 | else if (strcmp(key,"FREQ")==0) FREQ = val; 102 | else if (strcmp(key,"AXIS")==0) AXIS = val; 103 | } 104 | // 105 | }; 106 | const char CONFIG::JSON[] = R"( 107 | { 108 | 'MODE':[0,1,1,%d,'drift,stunt',0], 109 | 'KG':[0,100,1,%d,'%%',0], 110 | 'KP':[0,100,1,%d,'%%',0], 111 | 'KI':[0,100,1,%d,'%%',0], 112 | 'KD':[0,100,1,%d,'%%',0], 113 | 'REV':[0,1,1,%d,'bool',0], 114 | 'MIN':[1000,2000,1,%d,'usec',1], 115 | 'MAX':[1000,2000,1,%d,'usec',1], 116 | 'MEAN':[1000,2000,1,%d,'usec',1], 117 | 'ROLL':[0,90,1,%d,'deg',1], 118 | 'FREQ':[50,400,50,%d,'Hz',0], 119 | 'AXIS':[1,6,1,%d,'1-6',0], 120 | 'CH1_FREQ':[0,400,1,50,'Hz',2], 121 | 'CH1_USEC':[1000,2000,1,1500,'usec',2], 122 | 'IMU_PITCH':[-90,90,1,0,'deg',2], 123 | 'IMU_ROLL':[-90,90,1,0,'deg',2], 124 | 'IMU_RATE':[-360,360,1,0,'deg/sec',2], 125 | 'PID_LOOP':[0,500,1,50,'Hz',2], 126 | 'PID_USEC':[1000,2000,1,1500,'usec',2], 127 | } 128 | )"; 129 | 130 | 131 | 132 | //////////////////////////////////////////////////////////////////////////////// 133 | // class SERVER{}: WiFi/WWWサーバの管理クラス 134 | // setup(): サーバの初期化 135 | // start(): サーバの起動 136 | // loop(): サーバの処理 137 | // stop(): サーバの停止 138 | // isWake(): サーバの起動有無 139 | // lookFloat(): Ajax監視対象の登録 140 | //////////////////////////////////////////////////////////////////////////////// 141 | class SERVER { 142 | // 143 | static const char _SSID_[]; 144 | static WebServer server; 145 | static bool serverInit; 146 | static bool serverWake; 147 | // 148 | static const char HTML_INIT[]; 149 | static const char HTML_SAVE[]; 150 | static char CHAR_BUFF[]; 151 | // 152 | #define LOOK_MAX 8 153 | static int LOOK_INDEX; 154 | static char* LOOK_KEY[]; 155 | static float* LOOK_PTR[]; 156 | // 157 | static void handleRoot() { 158 | sprintf(CHAR_BUFF, HTML_INIT, CONF.getJSON()); 159 | server.send(200, "text/html", CHAR_BUFF); 160 | //DEBUG.println(CHAR_BUFF); 161 | } 162 | static void handleSave() { 163 | for (int n = 0; n < server.args(); n++) CONF.setCONF(server.argName(n).c_str(),server.arg(n).toInt()); 164 | CONF.save(); 165 | sprintf(CHAR_BUFF, HTML_SAVE, CONF.getJSON()); 166 | server.send(200, "text/html", CHAR_BUFF); 167 | delay(500); stop(); 168 | //DEBUG.println(CHAR_BUFF); 169 | } 170 | static void handleSaveOnly() { 171 | for (int n = 0; n < server.args(); n++) CONF.setCONF(server.argName(n).c_str(),server.arg(n).toInt()); 172 | CONF.save(); 173 | sprintf(CHAR_BUFF, HTML_INIT, CONF.getJSON()); 174 | server.send(200, "text/html", CHAR_BUFF); 175 | //delay(500); stop(); 176 | //DEBUG.println(CHAR_BUFF); 177 | } 178 | static void handleJson() { 179 | char* p = CHAR_BUFF; 180 | p = p + sprintf(p, "{"); 181 | for (int n = 0; n < LOOK_INDEX; n++) { 182 | p = p + sprintf(p,"\"%s\":%.f,", LOOK_KEY[n],*LOOK_PTR[n]); 183 | } 184 | sprintf(--p, "}"); 185 | server.send(200, "application/json", CHAR_BUFF); 186 | //DEBUG.println(CHAR_BUFF); 187 | } 188 | static void handleNotFound() { 189 | server.send(404, "text/plain", "Not Found."); 190 | } 191 | // 192 | public: 193 | // 194 | static CONFIG CONF; 195 | // 196 | static void setup(void) { 197 | CONF.setup(); 198 | } 199 | // 200 | static void start(void) { 201 | 202 | DEBUG.println("Setup WIFI AP mode"); 203 | WiFi.mode(WIFI_AP); 204 | WiFi.softAP(_SSID_); 205 | WiFi.begin(); 206 | delay(500); 207 | 208 | DEBUG.print("Started! IP address: "); 209 | DEBUG.println(WiFi.softAPIP()); 210 | 211 | if (MDNS.begin(_SSID_)) { 212 | MDNS.addService("http", "tcp", 80); 213 | DEBUG.println("MDNS responder started"); 214 | DEBUG.print("You can now connect to http://"); 215 | DEBUG.print(_SSID_); 216 | DEBUG.println(".local"); 217 | } 218 | 219 | if (!serverInit) { 220 | server.on("/", HTTP_GET, handleRoot); 221 | server.on("/json", HTTP_GET, handleJson); 222 | server.on("/save", HTTP_GET, handleSave); 223 | server.onNotFound(handleNotFound); 224 | server.begin(); 225 | DEBUG.println("HTTP server started"); 226 | serverInit = true; 227 | } 228 | 229 | serverWake = true; 230 | } 231 | static void loop(void) { 232 | if (serverWake) server.handleClient(); 233 | } 234 | static void stop(void) { 235 | if (serverWake) { 236 | WiFi.disconnect(); 237 | WiFi.mode(WIFI_OFF); 238 | serverWake = false; 239 | DEBUG.println("Stopped WIFI"); 240 | } 241 | } 242 | static bool isWake(void) { 243 | return serverWake; 244 | } 245 | static IPAddress getIP(void) { 246 | return WiFi.softAPIP(); 247 | } 248 | // 249 | static void lookFloat(const char *key, float *ptr) { 250 | if (LOOK_INDEX < LOOK_MAX) { 251 | LOOK_KEY[LOOK_INDEX] = (char*)key; 252 | LOOK_PTR[LOOK_INDEX] = ptr; 253 | LOOK_INDEX++; 254 | } 255 | } 256 | // 257 | }; 258 | // 259 | const char SERVER::_SSID_[] = "m5atom"; 260 | WebServer SERVER::server(80); 261 | bool SERVER::serverInit = false; 262 | bool SERVER::serverWake = false; 263 | // 264 | int SERVER::LOOK_INDEX = 0; 265 | char* SERVER::LOOK_KEY[LOOK_MAX]; 266 | float* SERVER::LOOK_PTR[LOOK_MAX]; 267 | // 268 | CONFIG SERVER::CONF; 269 | char SERVER::CHAR_BUFF[5000]; 270 | // 271 | const char SERVER::HTML_INIT[] = R"( 272 | 273 | 274 | 275 | 276 | GyroM5Atom 277 | 278 | 279 | 280 |
281 | 282 | 283 | 284 |
selectnamevaluesliderunit
285 |
286 | 287 | 288 | 289 |
290 | 291 |
292 | 293 | 400 | 401 | )"; 402 | 403 | 404 | const char SERVER::HTML_SAVE[] = R"( 405 | 406 | 407 | 408 | 409 | GyroM5Atom 410 | 411 | 412 |

GyroM5Atom

413 | parameters: 414 |
    415 |
416 | successfully saved! 417 | 418 | 436 | 437 | )"; 438 | 439 | 440 | 441 | 442 | //////////////////////////////////////////////////////////////////////////////// 443 | // class TimerMS{}: タイマ管理用ライブラリ 444 | // isUp(): 指定時間の経過有無（タイマ更新あり） 445 | // getFreq(): タイマ周期の参照[Hz] 446 | // getDelta(): タイマ経過の参照[msec] 447 | // touch(): タイマ更新（ウォッチドッグタイマ等で利用） 448 | // isOld(): 指定時間の経過有無（タイマ更新なし） 449 | //////////////////////////////////////////////////////////////////////////////// 450 | class TimerMS { 451 | unsigned long last; 452 | int freq; 453 | public: 454 | TimerMS() { 455 | last = 0; 456 | freq = 1; 457 | } 458 | 459 | bool isUp(int msec) { 460 | unsigned long now = millis(); 461 | if (last + msec <= now) { 462 | freq = 1000 / (now - last); 463 | last = now; 464 | return true; 465 | } 466 | return false; 467 | } 468 | int getFreq(void) { 469 | return freq; 470 | } 471 | int getDelta(void) { 472 | return millis() - last; 473 | } 474 | void touch(void) { 475 | last = millis(); 476 | } 477 | bool isOld(int msec) { 478 | unsigned long now = millis(); 479 | return last + msec <= now; 480 | } 481 | }; 482 | 483 | 484 | 485 | //////////////////////////////////////////////////////////////////////////////// 486 | // class CountHz{}: 周期計測用ライブラリ 487 | // getFreq(): 周期の取得[Hz] 488 | // touch(): カウント（例：ループ内で呼ぶ） 489 | //////////////////////////////////////////////////////////////////////////////// 490 | class CountHZ { 491 | unsigned long last; 492 | int freq, loop; 493 | public: 494 | CountHZ() { 495 | last = 0; 496 | freq = 0; 497 | loop = 0; 498 | } 499 | void touch(void) { 500 | unsigned long now = millis(); 501 | loop++; 502 | if (last + 1000 <= now) { 503 | last = now; 504 | freq = loop; 505 | loop = 0; 506 | } 507 | } 508 | int getFreq(void) { 509 | unsigned long now = millis(); 510 | return last + 1100 < now? 0: freq; 511 | } 512 | }; 513 | 514 | 515 | 516 | 517 | 518 | //////////////////////////////////////////////////////////////////////////////// 519 | // class PulsePort{}: PWM信号の入出力ライブラリ 520 | // setupIn(): 入力ピンの初期化 521 | // getUsec(): 入力パルス幅[usec] 522 | // getFreq(): 入力パルス周波数[Hz] 523 | // setupMean(): 入力パルス平均 524 | // getUsecMean(): 入力パスル平均[usec] 525 | // attach(): 割り込み処理の再開 526 | // detach(): 割り込み処理の中止 527 | // setupOut(): 出力ピンの初期化 528 | // putUsec(): 出力パルス幅[usec] 529 | // putFreq(): 出力パルス周波数[Hz] 530 | //////////////////////////////////////////////////////////////////////////////// 531 | // PWM watch dog timer 532 | #include 533 | 534 | // PWM pulse in 535 | typedef struct { 536 | int pin; 537 | int tout; 538 | // for pulse 539 | int dstUsec; 540 | int prev; 541 | unsigned long last; 542 | // for freq 543 | int dstFreq; 544 | unsigned long lastFreq; 545 | } InPulse; 546 | 547 | // PWM pulse out 548 | typedef struct { 549 | int pin; 550 | int freq; 551 | int bits; 552 | int duty; 553 | int usec; 554 | int dstUsec; 555 | } OutPulse; 556 | 557 | class PulsePort { 558 | static const int MAX = 4; // max of channels 559 | 560 | static int InCH; // number of in-channels 561 | static int OutCH; // number of out-channels 562 | 563 | static InPulse IN[MAX]; // pwm in-pulse 564 | static OutPulse OUT[MAX]; // pwm out-pulse 565 | 566 | static Ticker WDT; // watch dog timer 567 | static bool WATCHING; 568 | static float MEAN[MAX]; // mean of pwm in-pulse 569 | 570 | static void ISR(void *arg) { 571 | unsigned long tnow = micros(); 572 | int ch = (int)arg; 573 | InPulse* pwm = &IN[ch]; 574 | int vnow = digitalRead(pwm->pin); 575 | 576 | if (pwm->prev==0 && vnow==1) { 577 | // at up edge 578 | pwm->prev = 1; 579 | pwm->last = tnow; 580 | // for freq 581 | pwm->dstFreq = (tnow > pwm->lastFreq? 1000000/(tnow - pwm->lastFreq): 0); 582 | pwm->lastFreq = tnow; 583 | } 584 | else 585 | if (pwm->prev==1 && vnow==0) { 586 | // at down edge 587 | pwm->dstUsec = tnow - pwm->last; 588 | pwm->prev = 0; 589 | pwm->last = tnow; 590 | } 591 | } 592 | 593 | static void TSR(void) { 594 | unsigned long tnow = micros(); 595 | for (int ch=0; chlast + pwm->tout < tnow) { 598 | pwm->dstUsec = 0; 599 | pwm->dstFreq = 0; 600 | } 601 | } 602 | } 603 | 604 | public: 605 | PulsePort() { 606 | // do nothing 607 | }; 608 | static int setupIn(int pin, int toutUs=21*1000) { 609 | int ch = -1; 610 | if (InCH < MAX) { 611 | ch = InCH++; 612 | InPulse* pwm = &IN[ch]; 613 | // 614 | pwm->pin = pin; 615 | pwm->tout = toutUs; 616 | // for pulse 617 | pwm->dstUsec = 0; 618 | pwm->prev = 0; 619 | pwm->last = micros(); 620 | // for freq 621 | pwm->dstFreq = 0; 622 | pwm->lastFreq = micros(); 623 | // 624 | pinMode(pin,INPUT); 625 | attachInterruptArg(pin,&ISR,(void*)ch,CHANGE); 626 | if (ch == 0) WDT.attach_ms(pwm->tout/1000,&TSR); 627 | WATCHING = true; 628 | } 629 | return ch; 630 | }; 631 | static int getUsec(int ch) { 632 | if (ch >= 0 && ch < InCH) { 633 | InPulse* pwm = &IN[ch]; 634 | return pwm->dstUsec; 635 | } 636 | return -1; 637 | } 638 | static int getFreq(int ch) { 639 | if (ch >= 0 && ch < InCH) { 640 | InPulse* pwm = &IN[ch]; 641 | return pwm->dstFreq; 642 | } 643 | return -1; 644 | } 645 | 646 | static void detach(void) { 647 | if (InCH == 0) return; 648 | WDT.detach(); 649 | for (int ch=0; chpin); 652 | } 653 | WATCHING = false; 654 | }; 655 | static void attach(void) { 656 | if (InCH == 0 || WATCHING) return; 657 | for (int ch=0; chpin,&ISR,(void*)ch,CHANGE); 660 | if (ch == 0) WDT.attach_ms(pwm->tout/1000,&TSR); 661 | } 662 | WATCHING = true; 663 | }; 664 | 665 | // ESP32's PWM channel 0 and 1 have common frequency. 666 | static inline int CH2PWM(int ch) { return (ch)*2; }; 667 | 668 | static int setupOut(int pin, int freq = 50, int bits = 16) { 669 | int ch = -1; 670 | if (OutCH < MAX) { 671 | ch = OutCH++; 672 | OutPulse* out = &OUT[ch]; 673 | out->pin = pin; 674 | out->freq = freq; 675 | out->bits = bits; 676 | out->duty = (1 << bits); 677 | out->usec = 1000000/freq; 678 | // 679 | pinMode(out->pin,OUTPUT); 680 | ledcSetup(CH2PWM(ch),out->freq,out->bits); 681 | ledcWrite(CH2PWM(ch),0); 682 | ledcAttachPin(out->pin,CH2PWM(ch)); 683 | //DEBUG.printf("setupOut: ch=%d freq=%d bits=%d usec=%d\n",ch,out->freq,out->bits,out->usec); 684 | } 685 | return ch; 686 | } 687 | static float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) { 688 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 689 | } 690 | static bool putUsec(int ch, float usec) { 691 | if (ch >= 0 && ch < OutCH) { 692 | OutPulse* out = &OUT[ch]; 693 | int duty = mapFloat(usec, 0,out->usec, 0,out->duty); 694 | ledcWrite(CH2PWM(ch), duty); 695 | out->dstUsec = usec; 696 | return true; 697 | } 698 | return false; 699 | } 700 | static bool putFreq(int ch, int freq) { 701 | if (ch >= 0 && ch < OutCH) { 702 | OutPulse* out = &OUT[ch]; 703 | out->freq = freq; 704 | out->usec = 1000000/freq; 705 | // 706 | ledcWrite(CH2PWM(ch),0); 707 | ledcDetachPin(out->pin); 708 | ledcSetup(CH2PWM(ch),out->freq,out->bits); 709 | ledcWrite(CH2PWM(ch),0); 710 | ledcAttachPin(out->pin,CH2PWM(ch)); 711 | //DEBUG.printf("putFreq: ch=%d freq=%d bits=%d usec=%d\n",ch,out->freq,out->bits,out->usec); 712 | return true; 713 | } 714 | return false; 715 | } 716 | 717 | static void setupMean(bool first = false, int msec = 1000) { 718 | if (first) { 719 | unsigned long int timeout = millis() + msec; 720 | int count = 0; 721 | for (int ch=0; ch millis()) { 723 | for (int ch=0; chpin,pwm->dstUsec,pwm->dstFreq); 745 | } 746 | for (int ch=0; chpin,out->dstUsec,out->freq); 749 | } 750 | } 751 | 752 | }; 753 | 754 | // initialization for static class member 755 | int PulsePort::InCH = 0; 756 | int PulsePort::OutCH = 0; 757 | 758 | InPulse PulsePort::IN[PulsePort::MAX]; 759 | OutPulse PulsePort::OUT[PulsePort::MAX]; 760 | 761 | Ticker PulsePort::WDT; 762 | bool PulsePort::WATCHING = false; 763 | float PulsePort::MEAN[PulsePort::MAX]; 764 | 765 | 766 | 767 | //////////////////////////////////////////////////////////////////////////////// 768 | // class ServoPID{}: PID（比例、積分、微分）制御アルゴリズム（QuickPIDのラッパ） 769 | // setup(): PID制御のパラメータ変更 770 | // loop(): PID制御の出力計算 771 | //////////////////////////////////////////////////////////////////////////////// 772 | #include 773 | 774 | class ServoPID { 775 | public: 776 | 777 | float Setpoint, Input, Output; 778 | float Min, Mean, Max; 779 | QuickPID* QPID; 780 | 781 | ServoPID(void) { 782 | // 783 | Setpoint = 0.0F; 784 | Input = 0.0F; 785 | Output = 0.0F; 786 | // 787 | Mean = 1500; 788 | Min = 1000 - Mean; 789 | Max = 2000 - Mean; 790 | // 791 | QPID = new QuickPID(&Input, &Output, &Setpoint, 1.0,0.0,0.0, QuickPID::Action::direct); 792 | QPID->SetAntiWindupMode(QuickPID::iAwMode::iAwClamp); 793 | QPID->SetMode(QuickPID::Control::automatic); 794 | QPID->SetOutputLimits(Min,Max); 795 | QPID->SetSampleTimeUs(1000000/50); 796 | } 797 | 798 | // PID setup 799 | void setup(float Kp, float Ki, float Kd, int MIN=1000, int MEAN=1500, int MAX=2000, int Hz=50) { 800 | Min = MIN - MEAN; 801 | Mean = MEAN; 802 | Max = MAX - MEAN; 803 | 804 | QPID->SetTunings(Kp,Ki,Kd); 805 | QPID->SetOutputLimits(Min,Max); 806 | QPID->SetSampleTimeUs(Hz>=50? 1000000/Hz: 1000000/50); 807 | } 808 | void setupT(float Kp, float Ti, float Td, int MIN=1000, int MEAN=1500, int MAX=2000, int Hz=50) { 809 | if (Ti <= 0.0) Ti = 1.0; 810 | float Ki = Kp/Ti; 811 | float Kd = Kp*Td; 812 | setup(Kp,Ki,Kd, MIN,MEAN,MAX,Hz); 813 | } 814 | void setupU(float Ku, float Tu, int MIN=1000, int MEAN=1500, int MAX=2000, int Hz=50) { 815 | if (Tu <= 0.0) Tu = 1.0; 816 | // Ziegler–Nichols method 817 | float Ti = 0.50*Tu; 818 | float Td = 0.125*Tu; 819 | float Kp = 0.60*Ku; 820 | float Ki = Kp/Ti; 821 | float Kd = Kp*Td; 822 | setup(Kp,Ki,Kd, MIN,MEAN,MAX,Hz); 823 | } 824 | 825 | // PID loop 826 | float loop(float SP, float PV) { 827 | // Compute PID 828 | Setpoint = (SP > 0? SP - Mean: 0.0); 829 | Input = PV; 830 | QPID->Compute(); 831 | return SP > 0? Mean + constrain(Output,Min,Max): 0; 832 | } 833 | 834 | // debug print 835 | void debug(void) { 836 | DEBUG.printf("%.2f %.2f %.2f\n", Setpoint,Input,Output); 837 | } 838 | 839 | }; 840 | 841 | 842 | 843 | 844 | //////////////////////////////////////////////////////////////////////////////// 845 | // class M5StackAHRS{}: 姿勢推定用ライブラリ（可変更新周期、座標変換などに対応） 846 | // setup(): AHRSの初期化 847 | // loop(): AHRSの更新 848 | // initAXIS(): 座標軸の変更（シャーシ固定系の変更） 849 | // initMEAN(): バイアスの更新（センサのキャリブレーション） 850 | //////////////////////////////////////////////////////////////////////////////// 851 | class M5StackAHRS { 852 | /* AHRS */ 853 | //#include 854 | //--------------------------------------------------------------------------------------------------- 855 | // Definitions 856 | 857 | //#define sampleFreq 25.0f // sample frequency in Hz 858 | float sampleFreq = 25.0; 859 | #define twoKpDef (2.0f * 1.0f) // 2 * proportional gain 860 | #define twoKiDef (2.0f * 0.0f) // 2 * integral gain 861 | 862 | //#define twoKiDef (0.0f * 0.0f) 863 | 864 | //--------------------------------------------------------------------------------------------------- 865 | // Variable definitions 866 | 867 | volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp) 868 | volatile float twoKi = twoKiDef; // 2 * integral gain (Ki) 869 | volatile float q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion of sensor frame relative to auxiliary frame 870 | volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki 871 | 872 | //--------------------------------------------------------------------------------------------------- 873 | // IMU algorithm update 874 | 875 | void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw) { 876 | float recipNorm; 877 | float halfvx, halfvy, halfvz; 878 | float halfex, halfey, halfez; 879 | float qa, qb, qc; 880 | 881 | 882 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 883 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 884 | 885 | // Normalise accelerometer measurement 886 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 887 | ax *= recipNorm; 888 | ay *= recipNorm; 889 | az *= recipNorm; 890 | 891 | // Estimated direction of gravity and vector perpendicular to magnetic flux 892 | halfvx = q1 * q3 - q0 * q2; 893 | halfvy = q0 * q1 + q2 * q3; 894 | halfvz = q0 * q0 - 0.5f + q3 * q3; 895 | 896 | 897 | 898 | // Error is sum of cross product between estimated and measured direction of gravity 899 | halfex = (ay * halfvz - az * halfvy); 900 | halfey = (az * halfvx - ax * halfvz); 901 | halfez = (ax * halfvy - ay * halfvx); 902 | 903 | // Compute and apply integral feedback if enabled 904 | if(twoKi > 0.0f) { 905 | integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki 906 | integralFBy += twoKi * halfey * (1.0f / sampleFreq); 907 | integralFBz += twoKi * halfez * (1.0f / sampleFreq); 908 | gx += integralFBx; // apply integral feedback 909 | gy += integralFBy; 910 | gz += integralFBz; 911 | } 912 | else { 913 | integralFBx = 0.0f; // prevent integral windup 914 | integralFBy = 0.0f; 915 | integralFBz = 0.0f; 916 | } 917 | 918 | // Apply proportional feedback 919 | gx += twoKp * halfex; 920 | gy += twoKp * halfey; 921 | gz += twoKp * halfez; 922 | } 923 | 924 | // Integrate rate of change of quaternion 925 | gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors 926 | gy *= (0.5f * (1.0f / sampleFreq)); 927 | gz *= (0.5f * (1.0f / sampleFreq)); 928 | qa = q0; 929 | qb = q1; 930 | qc = q2; 931 | q0 += (-qb * gx - qc * gy - q3 * gz); 932 | q1 += (qa * gx + qc * gz - q3 * gy); 933 | q2 += (qa * gy - qb * gz + q3 * gx); 934 | q3 += (qa * gz + qb * gy - qc * gx); 935 | 936 | // Normalise quaternion 937 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 938 | q0 *= recipNorm; 939 | q1 *= recipNorm; 940 | q2 *= recipNorm; 941 | q3 *= recipNorm; 942 | 943 | 944 | *pitch = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch 945 | *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll 946 | *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3); //yaw 947 | 948 | *pitch *= RAD_TO_DEG; 949 | *yaw *= RAD_TO_DEG; 950 | // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is 951 | // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 952 | // - http://www.ngdc.noaa.gov/geomag-web/#declination 953 | *yaw -= 8.5; 954 | *roll *= RAD_TO_DEG; 955 | 956 | ///Serial.printf("%f %f %f \r\n", pitch, roll, yaw); 957 | } 958 | 959 | //--------------------------------------------------------------------------------------------------- 960 | // Fast inverse square-root 961 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 962 | 963 | float invSqrt(float x) { 964 | float halfx = 0.5f * x; 965 | float y = x; 966 | #pragma GCC diagnostic ignored "-Wstrict-aliasing" 967 | long i = *(long*)&y; 968 | i = 0x5f3759df - (i>>1); 969 | y = *(float*)&i; 970 | #pragma GCC diagnostic warning "-Wstrict-aliasing" 971 | y = y * (1.5f - (halfx * y * y)); 972 | return y; 973 | } 974 | 975 | 976 | /* IMU values */ 977 | float accl[3] = {0.0,0.0,0.0}; 978 | float gyro[3] = {0.0,0.0,0.0}; 979 | /* IMU mean values */ 980 | float ACCL[3] = {0.0,0.0,0.0}; 981 | float GYRO[3] = {0.0,0.0,0.0}; 982 | /* IMU temp */ 983 | float temp = 0.0F; 984 | /* AHRS */ 985 | float pitch = 0.0F; 986 | float roll = 0.0F; 987 | float yaw = 0.0F; 988 | 989 | /* Time */ 990 | uint32_t Now = 0; 991 | uint32_t lastUpdate = 0; 992 | float deltat = 1.0f; 993 | 994 | /* Axis = Body Fixed Frame */ 995 | float X[3] = {1.0,0.0,0.0}; 996 | float Y[3] = {0.0,1.0,0.0}; 997 | float Z[3] = {0.0,0.0,1.0}; 998 | 999 | /* LPF */ 1000 | //FilterLP LPF[6]; 1001 | 1002 | /* Vetor Operations */ 1003 | float dot(float* a,float* b) { return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]; } 1004 | float norm(float* a) { return sqrt(dot(a,a)); } 1005 | void mul(float* a,float k,float* b) { 1006 | b[0] = k*a[0]; 1007 | b[1] = k*a[1]; 1008 | b[2] = k*a[2]; 1009 | } 1010 | void add(float* a,float* b,float* c) { 1011 | c[0] = a[0] + b[0]; 1012 | c[1] = a[1] + b[1]; 1013 | c[2] = a[2] + b[2]; 1014 | } 1015 | void sub(float* a,float* b,float* c) { 1016 | c[0] = a[0] - b[0]; 1017 | c[1] = a[1] - b[1]; 1018 | c[2] = a[2] - b[2]; 1019 | } 1020 | void dup(float* a,float* b) { 1021 | b[0] = a[0]; 1022 | b[1] = a[1]; 1023 | b[2] = a[2]; 1024 | } 1025 | void normalize(float* a) { 1026 | float na = norm(a); 1027 | a[0] /= na; 1028 | a[1] /= na; 1029 | a[2] /= na; 1030 | } 1031 | void cross(float* a,float* b, float* c) { 1032 | c[0] = a[1]*b[2] - a[2]*b[1]; 1033 | c[1] = a[2]*b[0] - a[0]*b[2]; 1034 | c[2] = a[0]*b[1] - a[1]*b[0]; 1035 | } 1036 | 1037 | public: 1038 | /* initialize */ 1039 | void initMEAN(int msec = 2000) { 1040 | for (int i=0; i<3; i++) { 1041 | ACCL[i] = 0.0F; 1042 | GYRO[i] = 0.0F; 1043 | } 1044 | 1045 | int N = 0; 1046 | unsigned long int timeout = millis() + msec; 1047 | while (millis() < timeout) { 1048 | M5.IMU.getGyroData(&gyro[0],&gyro[1],&gyro[2]); 1049 | M5.IMU.getAccelData(&accl[0],&accl[1],&accl[2]); 1050 | for (int i=0; i<3; i++) { 1051 | ACCL[i] += accl[i]; 1052 | GYRO[i] += gyro[i]; 1053 | } 1054 | N++; 1055 | delay(1); 1056 | } 1057 | 1058 | for (int i=0; i<3; i++) { 1059 | ACCL[i] /= N; 1060 | GYRO[i] /= N; 1061 | } 1062 | } 1063 | 1064 | void initAXIS(int xdir = 1) { 1065 | // initial X0 1066 | int xabs = abs(xdir); 1067 | if (xabs>=1 && xabs<=3) { 1068 | X[0] = (xabs==1? xdir/xabs: 0); 1069 | X[1] = (xabs==2? xdir/xabs: 0); 1070 | X[2] = (xabs==3? xdir/xabs: 0); 1071 | } 1072 | 1073 | // Z := Anti Gravity 1074 | dup(ACCL,Z); 1075 | normalize(Z); 1076 | 1077 | // X := X0 - (Z,X0)Z 1078 | float zx = dot(Z,X); 1079 | float zxZ[3]; 1080 | mul(Z,zx,zxZ); 1081 | sub(X,zxZ,X); 1082 | normalize(X); 1083 | 1084 | // Y := Z x X 1085 | cross(Z,X,Y); 1086 | normalize(Y); 1087 | } 1088 | 1089 | void setup(int msec = 2000, int xdir = 1) { 1090 | M5.IMU.Init(); 1091 | initMEAN(msec); 1092 | initAXIS(xdir); 1093 | } 1094 | 1095 | void loop(float *gyro_=NULL, float *accl_=NULL, float *ahrs_=NULL, float *temp_=NULL) { 1096 | // put your main code here, to run repeatedly: 1097 | M5.IMU.getGyroData(&gyro[0], &gyro[1], &gyro[2]); 1098 | M5.IMU.getAccelData(&accl[0], &accl[1], &accl[2]); 1099 | M5.IMU.getTempData(&temp); 1100 | 1101 | // remove bias 1102 | for (int i=0; i<3; i++) gyro[i] -= GYRO[i]; 1103 | 1104 | // time update 1105 | Now = millis(); 1106 | deltat = ((Now - lastUpdate) / 1000.0); 1107 | lastUpdate = Now; 1108 | sampleFreq = 1.0/deltat; 1109 | 1110 | //M5.IMU.getAhrsData(&pitch,&roll,&yaw); 1111 | MahonyAHRSupdateIMU(dot(gyro,X)*DEG_TO_RAD,dot(gyro,Y)*DEG_TO_RAD,dot(gyro,Z)*DEG_TO_RAD, dot(accl,X),dot(accl,Y),dot(accl,Z), &pitch,&roll,&yaw); 1112 | 1113 | // copy results 1114 | if (gyro_) { 1115 | gyro_[0] = dot(gyro,X); 1116 | gyro_[1] = dot(gyro,Y); 1117 | gyro_[2] = dot(gyro,Z); 1118 | } 1119 | if (accl_) { 1120 | accl_[0] = dot(accl,X); 1121 | accl_[1] = dot(accl,Y); 1122 | accl_[2] = dot(accl,Z); 1123 | } 1124 | if (ahrs_) { 1125 | ahrs_[0] = roll; 1126 | ahrs_[1] = pitch; 1127 | ahrs_[2] = yaw; 1128 | } 1129 | if (temp_) { 1130 | *temp_ = temp; 1131 | } 1132 | } 1133 | void debug() { 1134 | DEBUG.println("M5StackAHRS:"); 1135 | DEBUG.printf(" gyro=(%.2f,%.2f,%.2f)\n",gyro[0],gyro[1],gyro[2]); 1136 | DEBUG.printf(" accl=(%.2f,%.2f,%.2f)\n",accl[0],accl[1],accl[2]); 1137 | DEBUG.printf(" ahrs=(%.2f,%.2f,%.2f)\n",roll,pitch,yaw); 1138 | DEBUG.printf(" GYRO=(%.2f,%.2f,%.2f)\n",GYRO[0],GYRO[1],GYRO[2]); 1139 | DEBUG.printf(" ACCL=(%.2f,%.2f,%.2f)\n",ACCL[0],ACCL[1],ACCL[2]); 1140 | DEBUG.printf(" X=(%.2f,%.2f,%.2f)\n",X[0],X[1],X[2]); 1141 | DEBUG.printf(" Y=(%.2f,%.2f,%.2f)\n",Y[0],Y[1],Y[2]); 1142 | DEBUG.printf(" Z=(%.2f,%.2f,%.2f)\n",Z[0],Z[1],Z[2]); 1143 | } 1144 | 1145 | int getFreq(void) { return int(1.0/deltat); } 1146 | #if 0 1147 | float getAccT(void) { return LPF[0].update(dot(accl,X)); } 1148 | float getAccL(void) { return LPF[1].update(dot(accl,Y)); } 1149 | float getAccV(void) { return dot(accl,Z); } 1150 | float getYawRate(void) { return LPF[2].update(dot(gyro,Z)); } 1151 | float getRoll(void) { 1152 | float Roll = - atan2(dot(accl,X),dot(accl,Z)); 1153 | return LPF[3].update(Roll * (180.0/PI)); 1154 | } 1155 | float getPitch(void) { 1156 | float Pitch = atan2(dot(accl,Y),dot(accl,Z)); 1157 | return LPF[4].update(Pitch * (180.0/PI)); 1158 | } 1159 | float getTraction(float G0, float y0=0.1) { 1160 | float x = getAccT()/G0; 1161 | return fabs(x)>=1.0? y0: sqrt(1.-x*x); 1162 | } 1163 | #endif 1164 | 1165 | }; 1166 | 1167 | 1168 | 1169 | //////////////////////////////////////////////////////////////////////////////// 1170 | // class M5AtomLED{}: LED制御ライブラリ（M5Atom標準ライブラリのバグ回避） 1171 | // setup(): 初期化 1172 | // blink(): 点滅 1173 | // fill(): 全面塗り 1174 | // setPixcel(): 一点塗り 1175 | //////////////////////////////////////////////////////////////////////////////// 1176 | #include 1177 | 1178 | class M5AtomLED { 1179 | CRGB LEDs[25]; 1180 | bool MASK[3][25]; 1181 | TimerMS TIME; 1182 | int state = 0; 1183 | public: 1184 | void setup(void) { 1185 | FastLED.addLeds(LEDs,25); 1186 | FastLED.setBrightness(20); 1187 | // 1188 | for (int p=0; p<25; p++) { 1189 | int x = p%5; 1190 | int y = p/5; 1191 | LEDs[p] = CRGB::Black; 1192 | MASK[0][p] = (x==2) && (y==2); 1193 | MASK[2][p] = (x==0) || (x==4) || (y==0) || (y==4); 1194 | MASK[1][p] = !(MASK[0][p] || MASK[2][p]); 1195 | } 1196 | state = 0; 1197 | // 1198 | for (int p=0; p<25; p++) { 1199 | int h = (p*360)/25; 1200 | LEDs[p] = CHSV(h,255,255); 1201 | } 1202 | FastLED.show(); 1203 | } 1204 | void blink(CRGB c, int msec) { 1205 | if (TIME.isUp(msec)) { 1206 | for (int p=0; p<25; p++) LEDs[p] = MASK[state][p]? c: CRGB::Black; 1207 | FastLED.show(); 1208 | state = (state + 1) % 3; 1209 | } 1210 | } 1211 | void fill(CRGB c, bool show=true) { 1212 | for (int p=0; p<25; p++) LEDs[p] = c; 1213 | if (show) FastLED.show(); 1214 | } 1215 | void setPixel(int p, CRGB c, bool show=true) { 1216 | LEDs[p%25] = c; 1217 | if (show) FastLED.show(); 1218 | } 1219 | void setPixel(int x, int y, CRGB c, bool show=true) { 1220 | setPixel(x+y*5,c,show); 1221 | } 1222 | 1223 | }; 1224 | 1225 | 1226 | 1227 | //////////////////////////////////////////////////////////////////////////////// 1228 | // EOF 1229 | //////////////////////////////////////////////////////////////////////////////// 1230 | -------------------------------------------------------------------------------- /GyroM5Atom/GyroM5Atom.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // ドリフトRCカー用ジャイロシステムGyroM5Atom 3 | // GyroM5Atom system for RC drift car 4 | // https://github.com/hshin-git/GyroM5 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include "GyroM5Atom.hpp" 9 | 10 | 11 | // GPIO of M5Atom 12 | const int BTM_PIN[] = {22,19,23,33}; 13 | const int GRV_PIN[] = {26,32}; 14 | 15 | 16 | // PWM I/O Ports 17 | PulsePort PWM_IO; 18 | 19 | 20 | // PID Controller 21 | ServoPID PID_CH1; 22 | 23 | 24 | // FREQ Counter 25 | CountHZ LOOP_HZ; 26 | 27 | // LED Matrix 28 | M5AtomLED M5_FACE; 29 | 30 | 31 | // AHRS of M5Atom 32 | M5StackAHRS M5_AHRS; 33 | float ACCL[3] = {0.0,0.0,0.0}; 34 | float GYRO[3] = {0.0,0.0,0.0}; 35 | float AHRS[3] = {0.0,0.0,0.0}; 36 | 37 | 38 | // CONFIG SERVER 39 | SERVER WWW; 40 | 41 | 42 | // SETTING PARAMETERS 43 | #define CNF_MODE (WWW.CONF.MODE) 44 | #define CNF_KG (WWW.CONF.KG/50.0 * 500./180.0) 45 | #define CNF_KP (WWW.CONF.KP/50.0) 46 | #define CNF_KI (WWW.CONF.KI/250.0) 47 | #define CNF_KD (WWW.CONF.KD/5000.0) 48 | #define CNF_REV (WWW.CONF.REV) 49 | #define CNF_MIN (WWW.CONF.MIN) 50 | #define CNF_MAX (WWW.CONF.MAX) 51 | #define CNF_MEAN (WWW.CONF.MEAN) 52 | #define CNF_ROLL (WWW.CONF.ROLL) 53 | #define CNF_FREQ (WWW.CONF.FREQ) 54 | #define CNF_AXIS (WWW.CONF.AXIS%2? (1+(WWW.CONF.AXIS-1)/2): -(1+(WWW.CONF.AXIS-1)/2)) 55 | 56 | #define COL_MODE (CNF_MODE==0? CRGB::Green : CRGB::Blue) 57 | 58 | 59 | // MONITORING VARIABLES 60 | float CH1_FREQ = 50; 61 | float CH1_USEC = 1500; 62 | float PID_LOOP = 100; 63 | float PID_USEC = 1500; 64 | float IMU_PITCH = 0; 65 | float IMU_ROLL = 0; 66 | float IMU_RATE = 0; 67 | 68 | 69 | void setup() 70 | { 71 | 72 | // put your setup code here, to run once: 73 | M5.begin(true,true,false); //Init M5Atom-Matrix(Serial, I2C, LEDs). 74 | M5.IMU.Init(); 75 | 76 | // FACE 77 | M5_FACE.setup(); 78 | 79 | // CONF 80 | WWW.setup(); 81 | WWW.lookFloat("CH1_FREQ",&CH1_FREQ); 82 | WWW.lookFloat("CH1_USEC",&CH1_USEC); 83 | WWW.lookFloat("IMU_PITCH",&IMU_PITCH); 84 | WWW.lookFloat("IMU_ROLL",&IMU_ROLL); 85 | WWW.lookFloat("IMU_RATE",&IMU_RATE); 86 | WWW.lookFloat("PID_LOOP",&PID_LOOP); 87 | WWW.lookFloat("PID_USEC",&PID_USEC); 88 | 89 | // AHRS 90 | M5_AHRS.setup(1000,CNF_AXIS); 91 | 92 | // PID 93 | PID_CH1.setup(CNF_KP,CNF_KI,CNF_KD,CNF_MIN,CNF_MEAN,CNF_MAX,400); 94 | 95 | // GPIO 96 | #if 1 97 | PWM_IO.setupIn(GRV_PIN[0]); 98 | PWM_IO.setupOut(GRV_PIN[1],CNF_FREQ); 99 | #else 100 | PWM_IO.setupIn(BTM_PIN[0]); 101 | PWM_IO.setupOut(BTM_PIN[1],CNF_FREQ); 102 | #endif 103 | 104 | } 105 | 106 | void loop() 107 | { 108 | 109 | // put your main code here, to run repeatedly: 110 | M5_AHRS.loop(GYRO,ACCL,AHRS); 111 | LOOP_HZ.touch(); 112 | // 113 | IMU_PITCH = AHRS[0]; 114 | IMU_ROLL = AHRS[1]; 115 | IMU_RATE = GYRO[2]; 116 | CH1_FREQ = PWM_IO.getFreq(0); 117 | CH1_USEC = PWM_IO.getUsec(0); 118 | PID_LOOP = LOOP_HZ.getFreq(); 119 | // 120 | if (CNF_MODE == 0) { 121 | PID_USEC = PID_CH1.loop(CH1_USEC, CNF_KG*(CNF_REV? -IMU_RATE: IMU_RATE)); 122 | } else 123 | if (CNF_MODE == 1 && abs(IMU_ROLL) > 30) { 124 | float DEL_ROLL = (IMU_ROLL>0? IMU_ROLL-CNF_ROLL: IMU_ROLL+CNF_ROLL); 125 | PID_USEC = PID_CH1.loop(CH1_USEC, 10*CNF_KG*(CNF_REV? -DEL_ROLL: DEL_ROLL)); 126 | } else 127 | { 128 | PID_USEC = CH1_USEC; 129 | } 130 | // 131 | PWM_IO.putUsec(0, (CH1_USEC>0? PID_USEC: CH1_USEC)); 132 | M5_FACE.blink(COL_MODE, (abs(IMU_ROLL) > 30? 200: 500)); 133 | 134 | // config 135 | M5.update(); 136 | if (M5.Btn.wasPressed() & !WWW.isWake()) { 137 | WWW.start(); 138 | while (WWW.isWake()) { 139 | WWW.loop(); 140 | // 141 | M5_AHRS.loop(GYRO,ACCL,AHRS); 142 | LOOP_HZ.touch(); 143 | // 144 | IMU_PITCH = AHRS[0]; 145 | IMU_ROLL = AHRS[1]; 146 | IMU_RATE = GYRO[2]; 147 | CH1_FREQ = PWM_IO.getFreq(0); 148 | CH1_USEC = PWM_IO.getUsec(0); 149 | //PID_LOOP = LOOP_HZ.getFreq(); 150 | PID_USEC = PID_CH1.loop(CH1_USEC,(CNF_REV? -CNF_KG*IMU_RATE: CNF_KG*IMU_RATE)); 151 | // 152 | PWM_IO.putUsec(0, CH1_USEC); 153 | M5_FACE.blink(CRGB::Yellow,500); 154 | // 155 | M5.update(); 156 | if (M5.Btn.wasPressed()) WWW.stop(); 157 | }; 158 | PID_CH1.setup(CNF_KP,CNF_KI,CNF_KD,CNF_MIN,CNF_MEAN,CNF_MAX,400); 159 | PWM_IO.putFreq(0,CNF_FREQ); 160 | M5_AHRS.setup(1000,CNF_AXIS); 161 | DEBUG.print("AXIS = "); DEBUG.println(CNF_AXIS); 162 | } 163 | 164 | } 165 | -------------------------------------------------------------------------------- /GyroM5Stick/GyroM5Stick.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////// 2 | // GyroM5v2 - M5StickC project: 3 | // Steering assisting unit for RC drift car 4 | // New Features from GyroM5v1: 5 | // Parameter setting by WiFi AP and WWW server 6 | // Variable PWM/PID frequency 7 | // Continuous time PID controller 8 | // Automatic detection of vertical direction 9 | // URL: 10 | // https://github.com/hshin-git/GyroM5 11 | ////////////////////////////////////////////////// 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | ////////////////////////////////////////////////// 22 | // Global constants 23 | ////////////////////////////////////////////////// 24 | // WiFi parameters 25 | const char *WIFI_SSID = "GyroM5v2"; 26 | const char *WIFI_PASS = NULL; // (8 char or more for WPA2, NULL for open use) 27 | const IPAddress WIFI_IP(192,168,5,1); 28 | const IPAddress WIFI_SUBNET(255,255,255,0); 29 | WiFiServer WIFI_SERVER(80); 30 | 31 | // GPIO parameters 32 | const int CH1_IN = 26; 33 | const int CH3_IN = 36; 34 | const int CH1_OUT = 0; // G0 must be HIGH while booting, so shoud be output pin 35 | 36 | // PWM channel 37 | const int PWM_CH1 = 0; 38 | const int PWM_CH2 = 1; 39 | 40 | // PWM resolution 41 | const int PWM_BITS = 16; // 16bit is valid up to 1220.70Hz 42 | const int PWM_DUTY = (1<400) return; 81 | PWM_FREQ = freq; 82 | PWM_USEC = 1000000/PWM_FREQ; 83 | // 84 | if (!firstTime) ledcDetachPin(CH1_OUT); 85 | ledcSetup(PWM_CH1,PWM_FREQ,PWM_BITS); 86 | ledcWrite(PWM_CH1,0); 87 | ledcAttachPin(CH1_OUT,PWM_CH1); 88 | firstTime = false; 89 | } 90 | void ch1_setUsec(int usec) { 91 | int duty = map(usec, 0,PWM_USEC, 0,PWM_DUTY); 92 | ledcWrite(PWM_CH1,(usec>0? duty: 0)); 93 | } 94 | 95 | 96 | 97 | ////////////////////////////////////////////////// 98 | // PWM reading without blocking 99 | ////////////////////////////////////////////////// 100 | // PWM watch dog timer 101 | Ticker PWMIN_WDT; 102 | // 103 | const int PWMIN_MAX = 4; 104 | int PWMIN_IDS = 0; 105 | typedef struct { 106 | int pin; 107 | int tout; 108 | // for pulse 109 | int *dst; 110 | int prev; 111 | unsigned long last; 112 | // for freq 113 | int *dstFreq; 114 | unsigned long lastFreq; 115 | } _PWMIN; 116 | _PWMIN PWMIN[PWMIN_MAX]; 117 | // PWM interrupt handler 118 | void _pwmin_isr(void *arg) { 119 | unsigned long tnow = micros(); 120 | int id = (int)arg; 121 | _PWMIN *pwm = &PWMIN[id]; 122 | int vnow = digitalRead(pwm->pin); 123 | if (pwm->prev==0 && vnow==1) { 124 | // at up edge 125 | pwm->prev = 1; 126 | pwm->last = tnow; 127 | // for freq 128 | *(pwm->dstFreq) = (tnow > pwm->lastFreq? 1000000/(tnow - pwm->lastFreq): 0); 129 | pwm->lastFreq = tnow; 130 | } 131 | else 132 | if (pwm->prev==1 && vnow==0) { 133 | // at down edge 134 | *(pwm->dst) = tnow - pwm->last; 135 | pwm->prev = 0; 136 | pwm->last = tnow; 137 | } 138 | } 139 | // PWM timer handler 140 | void _pwmin_tsr(void) { 141 | unsigned long tnow = micros(); 142 | for (int id=0; idlast + pwm->tout < tnow) { 145 | *(pwm->dst) = 0; 146 | *(pwm->dstFreq) = 0; 147 | } 148 | } 149 | } 150 | // 151 | bool pwmin_init(int pin, int *usec, int *freq, int toutUs=21*1000) { 152 | if (PWMIN_IDS < PWMIN_MAX) { 153 | int id = PWMIN_IDS; 154 | _PWMIN *pwm = &PWMIN[id]; 155 | // 156 | pwm->pin = pin; 157 | pwm->tout = toutUs; 158 | // for pulse 159 | pwm->dst = usec; 160 | pwm->prev = 0; 161 | pwm->last = micros(); 162 | // for freq 163 | pwm->dstFreq = freq; 164 | pwm->lastFreq = micros(); 165 | // 166 | pinMode(pin,INPUT); 167 | attachInterruptArg(pin,_pwmin_isr,(void*)id,CHANGE); 168 | if (id==0) PWMIN_WDT.attach_ms(pwm->tout/1000,_pwmin_tsr); 169 | // 170 | PWMIN_IDS = id + 1; 171 | return true; 172 | } 173 | return false; 174 | } 175 | // 176 | void pwmin_disable(void) { 177 | if (PWMIN_IDS <= 0) return; 178 | PWMIN_WDT.detach(); 179 | for (int id=0; idpin); 182 | } 183 | delay(GUI_MSEC); 184 | } 185 | void pwmin_enable(void) { 186 | if (PWMIN_IDS <= 0) return; 187 | for (int id=0; idpin,_pwmin_isr,(void*)id,CHANGE); 190 | if (id==0) PWMIN_WDT.attach_ms(pwm->tout/1000,_pwmin_tsr); 191 | } 192 | } 193 | 194 | 195 | ////////////////////////////////////////////////// 196 | // LCD helpers 197 | ////////////////////////////////////////////////// 198 | // Double bufferd LCD 199 | TFT_eSprite canvas = TFT_eSprite(&M5.Lcd); 200 | // 201 | void canvas_init(void) { 202 | M5.Axp.ScreenBreath(LCD_BACK); 203 | M5.Lcd.setRotation(0); 204 | canvas.createSprite(M5.Lcd.width(),M5.Lcd.height()); 205 | } 206 | bool canvas_header(char *text, int msec) { 207 | static unsigned long lastTime = 0; 208 | if (lastTime + msec < millis()) { 209 | M5.Rtc.GetTime(&RTC_TIME); 210 | canvas.fillScreen(BG_COLOR); 211 | canvas.setCursor(0,0); 212 | canvas.setTextColor(BG_COLOR,FG_COLOR); 213 | //canvas.printf(" %-12s\n",text); 214 | canvas.printf(" %-6s %02d:%02d\n",text,RTC_TIME.Hours,RTC_TIME.Minutes); 215 | canvas.setTextColor(FG_COLOR,BG_COLOR); 216 | lastTime = millis(); 217 | return true; 218 | } 219 | return false; 220 | } 221 | void canvas_footer(char *text) { 222 | M5.Rtc.GetData(&RTC_DATE); 223 | canvas.setTextColor(BG_COLOR,FG_COLOR); 224 | //canvas.printf(" %-12s\n",text); 225 | canvas.printf(" %-6s %02d/%02d\n",text,RTC_DATE.Month,RTC_DATE.Date); 226 | canvas.setTextColor(FG_COLOR,BG_COLOR); 227 | canvas.pushSprite(0,0); 228 | } 229 | 230 | 231 | 232 | ////////////////////////////////////////////////// 233 | // Ring buffer to store and draw sampled values 234 | ////////////////////////////////////////////////// 235 | // Ring buffer 236 | const int RING_MAX = 4; 237 | int RING_IDS = 0; 238 | typedef struct { 239 | int *buff; 240 | int head; 241 | char *text; 242 | int color; 243 | } _RING; 244 | _RING RING[RING_MAX]; 245 | 246 | // data storage 247 | const int DATA_MSEC = 100; // sampling cycle in msec 248 | const int DATA_SIZE = 8*60*(1000/DATA_MSEC); // sampling storage size 249 | // 250 | int DATA_Setpoint[DATA_SIZE]; 251 | int DATA_Output[DATA_SIZE]; 252 | int DATA_Input[DATA_SIZE]; 253 | 254 | // data operations 255 | int data_init(int *buf, char *txt, int col=TFT_WHITE) { 256 | int id = RING_IDS; 257 | if (id < RING_MAX) { 258 | RING[id].buff = buf; 259 | RING[id].head = 0; 260 | RING[id].text = txt; 261 | RING[id].color = col; 262 | for (int i=0; i0? min(N,lastData): N); 278 | 279 | for (int id=0; idprint("SEC,"); 316 | for (int id=0; idprint(RING[id].text); 318 | cl->print(idprintf("%.3f,", n*DATA_MSEC/1000.0); 323 | for (int id=0; idprint(A[p]); 326 | cl->print(id0? min(N,lastData): N); 345 | float MAE = 0.0; 346 | p = (p-LAST+N)%N; 347 | for (int n=0; n0? min(N,lastData): N); 359 | float MSE = 0.0; 360 | p = (p-LAST+N)%N; 361 | for (int n=0; nprint(KEYS[n]); 447 | cl->print(nprint(CONFIG[n]); 452 | cl->print(n 485 | 486 | 487 | 488 | %s 489 | 490 | 491 |
492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 |
namerangevaluedescription
KG0IMU gain G (0-100)
KP0PID gain P (0-100)
KI0PID gain I (0-100)
KD0PID gain D (0-100)
CH100:NOR, 1:REV
CH300:TB, 1:KG, 2:KP, 3:KI, 4:KD, 5:NO
PWM50PWM frequency (Hz)
502 | 503 | 504 | 505 | 506 |
507 | 508 | 524 | 525 | )"; 526 | 527 | // HTML buffer 528 | char HTML_BUFFER[sizeof(HTML_TEMPLATE)+sizeof(WIFI_SSID)+8*SIZE]; 529 | 530 | // foward prototype 531 | void gpid_init(bool); 532 | 533 | // Web server for config 534 | bool configAccepted = false; 535 | void serverLoop() { 536 | WiFiClient client = WIFI_SERVER.available(); 537 | 538 | if (client) { 539 | //Serial.println("New Client."); 540 | String currentLine = ""; 541 | 542 | while (client.connected()) { 543 | if (client.available()) { 544 | char c = client.read(); 545 | //Serial.write(c); 546 | if (c == '\n') { 547 | if (currentLine.length() == 0) { 548 | // response for request "/" 549 | client.println("HTTP/1.1 200 OK"); 550 | client.println("Content-type:text/html; charset=utf-8;"); 551 | client.println(""); 552 | sprintf(HTML_BUFFER,HTML_TEMPLATE, WIFI_SSID,CONFIG[_KG],CONFIG[_KP],CONFIG[_KI],CONFIG[_KD],CONFIG[_CH1],CONFIG[_CH3],CONFIG[_PWM],0); 553 | client.println(HTML_BUFFER); 554 | break; 555 | } 556 | else 557 | if (currentLine.indexOf("GET /?") == 0) { 558 | // response for request "/?KG=..." 559 | int p1 = 0; 560 | int p2 = 0; 561 | int val = 0; 562 | // set CONFIG 563 | for (int n=0; n<(SIZE-TAIL); n++) { 564 | char key[16]; 565 | sprintf(key,"%s=",KEYS[n]); 566 | p1 = currentLine.indexOf(key, p2) + strlen(key); 567 | p2 = currentLine.indexOf('&', p1); 568 | val = currentLine.substring(p1, p2).toInt(); 569 | CONFIG[n] = val; 570 | } 571 | // set RTC 572 | p1 = currentLine.indexOf("JST=",p2) + strlen("JST="); 573 | RTC_DATE.Year = currentLine.substring(p1+0,p1+4).toInt(); 574 | RTC_DATE.Month = currentLine.substring(p1+4,p1+6).toInt(); 575 | RTC_DATE.Date = currentLine.substring(p1+6,p1+8).toInt(); 576 | M5.Rtc.SetData(&RTC_DATE); 577 | RTC_TIME.Hours = currentLine.substring(p1+8,p1+10).toInt(); 578 | RTC_TIME.Minutes = currentLine.substring(p1+10,p1+12).toInt(); 579 | RTC_TIME.Seconds = currentLine.substring(p1+12,p1+14).toInt(); 580 | M5.Rtc.SetTime(&RTC_TIME); 581 | // save CONFIG 582 | config_puts(); 583 | ch1_setFreq(CONFIG[_PWM]); 584 | gpid_init(true); 585 | // response 586 | client.println("HTTP/1.1 200 OK"); 587 | client.println("Content-type:text/html; charset=utf-8;"); 588 | client.println(""); 589 | sprintf(HTML_BUFFER,HTML_TEMPLATE, WIFI_SSID,CONFIG[_KG],CONFIG[_KP],CONFIG[_KI],CONFIG[_KD],CONFIG[_CH1],CONFIG[_CH3],CONFIG[_PWM],1); 590 | client.println(HTML_BUFFER); 591 | configAccepted = true; 592 | break; 593 | } 594 | else 595 | if (currentLine.indexOf("GET /csv") == 0) { 596 | // response for request "/csv" 597 | client.println("HTTP/1.1 200 OK"); 598 | client.println("Content-type:text/csv; charset=utf-8;"); 599 | //client.println("Content-Disposition:attachment; filename=data.csv"); 600 | client.printf("Content-Disposition:attachment; filename=data-%04d%02d%02d-%02d%02d.csv\n", RTC_DATE.Year,RTC_DATE.Month,RTC_DATE.Date, RTC_TIME.Hours,RTC_TIME.Minutes); 601 | client.println(""); 602 | config_dump(&client); 603 | data_dump(&client); 604 | client.println(""); 605 | break; 606 | } 607 | else 608 | { 609 | currentLine = ""; 610 | } 611 | } else if (c != '\r') { 612 | currentLine += c; 613 | } 614 | } 615 | } 616 | client.stop(); 617 | //Serial.println("Client Disconnected."); 618 | } 619 | } 620 | 621 | void setup_by_wifi() { 622 | // 623 | //WIFI_SERVER.begin(); 624 | // 625 | if (canvas_header("WIFI",0)) { 626 | char url[32]; 627 | canvas.println("[A] HOME"); 628 | canvas.println("SSID:"); canvas.printf(" %s\n",WIFI_SSID); 629 | canvas.println("PASS:"); canvas.printf(" %s\n",(WIFI_PASS==NULL? "": WIFI_PASS)); 630 | canvas.println("IP:"); canvas.print(" "); canvas.println(WIFI_IP); 631 | canvas_footer("WIFI"); 632 | sprintf(url,"http://%d.%d.%d.%d/",((WIFI_IP>>0)&0xff),((WIFI_IP>>8)&0xff),((WIFI_IP>>16)&0xff),((WIFI_IP>>24)&0xff)); 633 | M5.Lcd.qrcode(url,0,80,80,2); 634 | } 635 | delay(GUI_MSEC); 636 | // 637 | configAccepted = false; 638 | while (!configAccepted) { 639 | serverLoop(); 640 | vin_watch(); 641 | M5.update(); 642 | if (M5.BtnA.isPressed()) { 643 | delay(GUI_MSEC); 644 | return; 645 | } 646 | //delay(2); 647 | } 648 | // 649 | //WIFI_SERVER.end(); 650 | // 651 | } 652 | 653 | 654 | // config for ch1 end points 655 | void setup_ch1ends() { 656 | int ch1,val; 657 | for (int n=0; n<2; n++) { 658 | delay(GUI_MSEC); 659 | while (true) { 660 | ch1 = pulseIn(CH1_IN,HIGH,PWM_WAIT); 661 | val = map(ch1, 0,PWM_USEC, 0,PWM_DUTY); 662 | //ledcWrite(PWM_CH1,(ch1>0? val: 0)); 663 | ch1_setUsec(ch1); 664 | if (canvas_header("ENDS",LCD_MSEC)) { 665 | canvas.println((n? "LEFT": "RIGHT")); 666 | canvas.printf("[A] SAVE\n"); 667 | canvas.printf("[B] HOME\n"); 668 | canvas.printf(" CH1:%6d\n",ch1); 669 | canvas.printf(" VAL:%6d\n",val); 670 | canvas.printf(" |%s| \n",(n? "<<<< ": " >>>>")); 671 | canvas_footer("ENDS"); 672 | } 673 | M5.update(); 674 | if (M5.BtnA.isPressed()) { 675 | if (ch1>0) CONFIG[(n? _MAX: _MIN)] = ch1; 676 | break; 677 | } 678 | else 679 | if (M5.BtnB.isPressed()) { 680 | //ch1_setUsec(0); 681 | delay(GUI_MSEC); 682 | return; 683 | } 684 | } 685 | } 686 | // save 687 | if (ch1>0) { 688 | if (CONFIG[_MAX] < CONFIG[_MIN]) { 689 | int temp = CONFIG[_MAX]; 690 | CONFIG[_MAX] = CONFIG[_MIN]; 691 | CONFIG[_MIN] = temp; 692 | } 693 | config_puts(); 694 | } 695 | //ch1_setUsec(0); 696 | delay(GUI_MSEC); 697 | } 698 | 699 | 700 | 701 | ////////////////////////////////////////////////// 702 | // Low/High Pass Filters 703 | ////////////////////////////////////////////////// 704 | //float LPF_ME[4]; 705 | //float LPF_MAE[4]; 706 | //float LPF_MSE[4]; 707 | //// LPF 708 | //void lpf_init(float buf[],float alpha) { 709 | // buf[0] = 0.; 710 | // buf[1] = alpha; 711 | //} 712 | //float lpf_update(float buf[],float x) { 713 | // float yp = buf[0]; 714 | // float alpha = buf[1]; 715 | // float y = alpha*x + (1.-alpha)*yp; 716 | // buf[0] = y; 717 | // return y; 718 | //} 719 | //// HPF 720 | //void hpf_init(float buf[],float alpha) { 721 | // buf[0] = 0.; 722 | // buf[1] = 0.; 723 | // buf[2] = alpha; 724 | //} 725 | //float hpf_update(float buf[],float x) { 726 | // float yp = buf[0]; 727 | // float xp = buf[1]; 728 | // float alpha = buf[2]; 729 | // float y = alpha*(x - xp) + alpha*yp; 730 | // buf[0] = y; 731 | // buf[1] = x; 732 | // return y; 733 | //} 734 | 735 | 736 | 737 | ////////////////////////////////////////////////// 738 | // Initial calibrators 739 | ////////////////////////////////////////////////// 740 | float CH1US_MEAN; 741 | float OMEGA_MEAN[3]; 742 | float ACCEL_MEAN[3]; 743 | 744 | // Frequency counter 745 | int countHz(bool getOnly = false) { 746 | static unsigned long lastTime = 0; 747 | static int count = 0; 748 | static int freq = 50; 749 | if (!getOnly) count = count + 1; 750 | if (lastTime + 1000 < millis()) { 751 | freq = count; 752 | count = 0; 753 | lastTime = millis(); 754 | } 755 | return freq; 756 | } 757 | 758 | void mean_init(void) { 759 | unsigned long startTime; 760 | int count; 761 | // wait 762 | while (pulseIn(CH1_IN,HIGH,PWM_WAIT)==0) { 763 | vin_watch(); 764 | if (canvas_header("WAIT", LCD_MSEC)) { 765 | for (int n=0; n0? CH1_USEC - CH1US_MEAN: 0.0; 911 | Input = Kg * yrate; 912 | GyroPID.Compute(); 913 | ch1_usec = constrain(CH1US_MEAN + Output, CONFIG[_MIN],CONFIG[_MAX]); 914 | 915 | // Output PWM 916 | ch1_setUsec((CH1_USEC>0? ch1_usec: 0)); 917 | } 918 | // 919 | bool gpid_timing(int usec) { 920 | static unsigned long lastTime = 0; 921 | if (lastTime + usec < micros()) { 922 | lastTime = micros(); 923 | return true; 924 | } 925 | return false; 926 | } 927 | 928 | 929 | 930 | ////////////////////////////////////////////////// 931 | // put your setup code here, to run once: 932 | ////////////////////////////////////////////////// 933 | void setup() { 934 | // (1) setup M5StickC object 935 | M5.begin(); 936 | M5.IMU.Init(); 937 | //while (!setCpuFrequencyMhz(80)); 938 | 939 | // (2) setup configuration 940 | canvas_init(); 941 | config_init(); 942 | 943 | // (3) setup WiFi AP 944 | wifi_init(); 945 | 946 | // (4) setup GPIO 947 | pinMode(CH1_IN,INPUT); 948 | pinMode(CH3_IN,INPUT); 949 | pinMode(CH1_OUT,OUTPUT); 950 | pwmin_init(CH1_IN,&CH1_USEC,&CH1_FREQ,PWM_WAIT); 951 | pwmin_init(CH3_IN,&CH3_USEC,&CH3_FREQ,PWM_WAIT); 952 | ch1_setFreq(CONFIG[_PWM]); 953 | 954 | // (5) Initialize Ring buffer 955 | data_init(DATA_Setpoint,"CH1",TFT_CYAN); 956 | data_init(DATA_Output,"SRV",TFT_MAGENTA); 957 | data_init(DATA_Input,"YAW",TFT_YELLOW); 958 | 959 | // (6) setup Zeros/Means 960 | mean_init(); 961 | 962 | // (7) setup PID 963 | gpid_init(true); 964 | 965 | // (8) setup others 966 | //Serial.begin(115200); 967 | } 968 | 969 | 970 | 971 | ////////////////////////////////////////////////// 972 | // put your main code here, to run repeatedly: 973 | ////////////////////////////////////////////////// 974 | void loop() { 975 | // Fetch Setpoint/Input and compute Output by PID 976 | //CH1_USEC = pulseIn(CH1_IN,HIGH,PWM_WAIT); 977 | if (gpid_timing(PWM_USEC)) { 978 | gpid_update(); 979 | countHz(); 980 | } 981 | 982 | // Sample PID variables in every 100msec 983 | if (data_sample(DATA_MSEC)){ 984 | data_put(0,Setpoint); 985 | data_put(1,Output); 986 | data_put(2,Input); 987 | } 988 | 989 | // Monitor variables in every 500msec 990 | if (canvas_header("HOME",LCD_MSEC)) { 991 | int lastData = 8*1000/DATA_MSEC; 992 | int lastLine = 1; 993 | int ch3_gain; 994 | // RCV monitor 995 | //canvas.println("RCV (us)"); lastLine++; 996 | //canvas.printf( " CH1:%6d\n", CH1_USEC); lastLine++; 997 | //canvas.printf( " CH2:%6d\n", CH2_USEC); lastLine++; 998 | //canvas.printf( " CH3:%6d\n", CH3_USEC); lastLine++; 999 | // PWM monitor 1000 | canvas.println("PWM (Hz)"); lastLine++; 1001 | canvas.printf( " IN :%6d\n", CH1_FREQ); lastLine++; 1002 | canvas.printf( " OUT:%6d\n", PWM_FREQ); lastLine++; 1003 | canvas.printf( " PID:%6d\n", countHz(true)); lastLine++; 1004 | // IMU monitor 1005 | //canvas.println("OMEGA (rad/s)"); lastLine++; 1006 | //canvas.printf( " X:%8.2f\n", IMU_OMEGA[0]); lastLine++; 1007 | //canvas.printf( " Y:%8.2f\n", IMU_OMEGA[1]); lastLine++; 1008 | //canvas.printf( " Z:%8.2f\n", IMU_OMEGA[2]); lastLine++; 1009 | //canvas.println("ACCEL (G)"); lastLine++; 1010 | //canvas.printf( " X:%8.2f\n", IMU_ACCEL[0]); lastLine++; 1011 | //canvas.printf( " Y:%8.2f\n", IMU_ACCEL[1]); lastLine++; 1012 | //canvas.printf( " Z:%8.2f\n", IMU_ACCEL[2]); lastLine++; 1013 | // PID monitor 1014 | canvas.println("PID (0-100)"); lastLine++; 1015 | canvas.printf( " G/P:%3d/%3d\n", CONFIG[_KG],CONFIG[_KP]); lastLine++; 1016 | canvas.printf( " I/D:%3d/%3d\n", CONFIG[_KI],CONFIG[_KD]); lastLine++; 1017 | //canvas.printf( " MAE:%6.1f\n", data_MAE(0,2,lastData)); lastLine++; 1018 | //canvas.printf( "RMSE:%6.1f\n", data_RMSE(0,2,lastData)); lastLine++; 1019 | // RGB graph 1020 | data_grid(0); 1021 | data_grid(CONFIG[_MIN]-CH1US_MEAN); 1022 | data_grid(CONFIG[_MAX]-CH1US_MEAN); 1023 | data_draw(lastData,lastLine); 1024 | // LCD draw 1025 | canvas_footer("HOME"); 1026 | 1027 | // CH3 >> CONFIG 1028 | //CH3_USEC = pulseIn(CH3_IN,HIGH,PWM_WAIT); 1029 | if (CH3_USEC > 0) { 1030 | ch3_gain = map(CH3_USEC, PULSE_MIN,PULSE_MAX, -GAIN_AMP,GAIN_AMP); 1031 | ch3_gain = constrain(ch3_gain, GAIN_MIN,GAIN_MAX); 1032 | switch (CONFIG[_CH3]) { 1033 | case 1: CONFIG[_KG] = ch3_gain; break; 1034 | case 2: CONFIG[_KP] = ch3_gain; break; 1035 | case 3: CONFIG[_KI] = ch3_gain; break; 1036 | case 4: CONFIG[_KD] = ch3_gain; break; 1037 | case 5: CONFIG[_KG] = 50; CONFIG[_KG] = CONFIG[_KI] = CONFIG[_KD] = 0; break; 1038 | default: break; 1039 | } 1040 | } 1041 | // CONFIG >> QuickPID 1042 | gpid_init(); 1043 | } 1044 | 1045 | // Watch vin and buttons 1046 | vin_watch(); 1047 | M5.update(); 1048 | if (M5.BtnA.isPressed()) setup_by_wifi(); 1049 | else 1050 | if (M5.BtnB.isPressed()) setup_ch1ends(); 1051 | } 1052 | -------------------------------------------------------------------------------- /GyroM5StickPlus/GyroM5StickPlus.ino: -------------------------------------------------------------------------------- 1 | ////////////////////////////////////////////////// 2 | // GyroM5v2 - M5StickC project: 3 | // Steering assisting unit for RC drift car 4 | // New Features from GyroM5v1: 5 | // Parameter setting by WiFi AP and WWW server 6 | // Variable PWM/PID frequency 7 | // Continuous time PID controller 8 | // Automatic detection of vertical direction 9 | // URL: 10 | // https://github.com/hshin-git/GyroM5 11 | ////////////////////////////////////////////////// 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | ////////////////////////////////////////////////// 22 | // Global constants 23 | ////////////////////////////////////////////////// 24 | // WiFi parameters 25 | const char *WIFI_SSID = "GyroM5v2plus"; 26 | const char *WIFI_PASS = NULL; // (8 char or more for WPA2, NULL for open use) 27 | const IPAddress WIFI_IP(192,168,5,1); 28 | const IPAddress WIFI_SUBNET(255,255,255,0); 29 | WiFiServer WIFI_SERVER(80); 30 | 31 | // GPIO parameters 32 | const int CH1_IN = 26; 33 | const int CH3_IN = 36; 34 | const int CH1_OUT = 0; // G0 must be HIGH while booting, so shoud be output pin 35 | 36 | // PWM channel 37 | const int PWM_CH1 = 0; 38 | const int PWM_CH2 = 1; 39 | 40 | // PWM resolution 41 | const int PWM_BITS = 16; // 16bit is valid up to 1220.70Hz 42 | const int PWM_DUTY = (1<400) return; 81 | PWM_FREQ = freq; 82 | PWM_USEC = 1000000/PWM_FREQ; 83 | // 84 | if (!firstTime) ledcDetachPin(CH1_OUT); 85 | ledcSetup(PWM_CH1,PWM_FREQ,PWM_BITS); 86 | ledcWrite(PWM_CH1,0); 87 | ledcAttachPin(CH1_OUT,PWM_CH1); 88 | firstTime = false; 89 | } 90 | void ch1_setUsec(int usec) { 91 | int duty = map(usec, 0,PWM_USEC, 0,PWM_DUTY); 92 | ledcWrite(PWM_CH1,(usec>0? duty: 0)); 93 | } 94 | 95 | 96 | 97 | ////////////////////////////////////////////////// 98 | // PWM reading without blocking 99 | ////////////////////////////////////////////////// 100 | // PWM watch dog timer 101 | Ticker PWMIN_WDT; 102 | // 103 | const int PWMIN_MAX = 4; 104 | int PWMIN_IDS = 0; 105 | typedef struct { 106 | int pin; 107 | int tout; 108 | // for pulse 109 | int *dst; 110 | int prev; 111 | unsigned long last; 112 | // for freq 113 | int *dstFreq; 114 | unsigned long lastFreq; 115 | } _PWMIN; 116 | _PWMIN PWMIN[PWMIN_MAX]; 117 | // PWM interrupt handler 118 | void _pwmin_isr(void *arg) { 119 | unsigned long tnow = micros(); 120 | int id = (int)arg; 121 | _PWMIN *pwm = &PWMIN[id]; 122 | int vnow = digitalRead(pwm->pin); 123 | if (pwm->prev==0 && vnow==1) { 124 | // at up edge 125 | pwm->prev = 1; 126 | pwm->last = tnow; 127 | // for freq 128 | *(pwm->dstFreq) = (tnow > pwm->lastFreq? 1000000/(tnow - pwm->lastFreq): 0); 129 | pwm->lastFreq = tnow; 130 | } 131 | else 132 | if (pwm->prev==1 && vnow==0) { 133 | // at down edge 134 | *(pwm->dst) = tnow - pwm->last; 135 | pwm->prev = 0; 136 | pwm->last = tnow; 137 | } 138 | } 139 | // PWM timer handler 140 | void _pwmin_tsr(void) { 141 | unsigned long tnow = micros(); 142 | for (int id=0; idlast + pwm->tout < tnow) { 145 | *(pwm->dst) = 0; 146 | *(pwm->dstFreq) = 0; 147 | } 148 | } 149 | } 150 | // 151 | bool pwmin_init(int pin, int *usec, int *freq, int toutUs=21*1000) { 152 | if (PWMIN_IDS < PWMIN_MAX) { 153 | int id = PWMIN_IDS; 154 | _PWMIN *pwm = &PWMIN[id]; 155 | // 156 | pwm->pin = pin; 157 | pwm->tout = toutUs; 158 | // for pulse 159 | pwm->dst = usec; 160 | pwm->prev = 0; 161 | pwm->last = micros(); 162 | // for freq 163 | pwm->dstFreq = freq; 164 | pwm->lastFreq = micros(); 165 | // 166 | pinMode(pin,INPUT); 167 | attachInterruptArg(pin,_pwmin_isr,(void*)id,CHANGE); 168 | if (id==0) PWMIN_WDT.attach_ms(pwm->tout/1000,_pwmin_tsr); 169 | // 170 | PWMIN_IDS = id + 1; 171 | return true; 172 | } 173 | return false; 174 | } 175 | // 176 | bool gpio25_dis_init(){ 177 | gpio_pulldown_dis(GPIO_NUM_25); 178 | gpio_pullup_dis(GPIO_NUM_25); 179 | } 180 | // 181 | void pwmin_disable(void) { 182 | if (PWMIN_IDS <= 0) return; 183 | PWMIN_WDT.detach(); 184 | for (int id=0; idpin); 187 | } 188 | delay(GUI_MSEC); 189 | } 190 | void pwmin_enable(void) { 191 | if (PWMIN_IDS <= 0) return; 192 | for (int id=0; idpin,_pwmin_isr,(void*)id,CHANGE); 195 | if (id==0) PWMIN_WDT.attach_ms(pwm->tout/1000,_pwmin_tsr); 196 | } 197 | } 198 | 199 | 200 | ////////////////////////////////////////////////// 201 | // LCD helpers 202 | ////////////////////////////////////////////////// 203 | // Double bufferd LCD 204 | TFT_eSprite canvas = TFT_eSprite(&M5.Lcd); 205 | // 206 | void canvas_init(void) { 207 | M5.Axp.ScreenBreath(LCD_BACK); 208 | M5.Lcd.setRotation(0); 209 | canvas.createSprite(M5.Lcd.width(),M5.Lcd.height()); 210 | } 211 | bool canvas_header(char *text, int msec) { 212 | static unsigned long lastTime = 0; 213 | if (lastTime + msec < millis()) { 214 | M5.Rtc.GetTime(&RTC_TIME); 215 | canvas.fillScreen(BG_COLOR); 216 | canvas.setCursor(0,0); 217 | canvas.setTextColor(BG_COLOR,FG_COLOR); 218 | //canvas.printf(" %-12s\n",text); 219 | canvas.printf(" %-6s %02d:%02d\n",text,RTC_TIME.Hours,RTC_TIME.Minutes); 220 | canvas.setTextColor(FG_COLOR,BG_COLOR); 221 | lastTime = millis(); 222 | return true; 223 | } 224 | return false; 225 | } 226 | void canvas_footer(char *text) { 227 | M5.Rtc.GetData(&RTC_DATE); 228 | canvas.setTextColor(BG_COLOR,FG_COLOR); 229 | //canvas.printf(" %-12s\n",text); 230 | canvas.printf(" %-6s %02d/%02d\n",text,RTC_DATE.Month,RTC_DATE.Date); 231 | canvas.setTextColor(FG_COLOR,BG_COLOR); 232 | canvas.pushSprite(0,0); 233 | } 234 | 235 | 236 | 237 | ////////////////////////////////////////////////// 238 | // Ring buffer to store and draw sampled values 239 | ////////////////////////////////////////////////// 240 | // Ring buffer 241 | const int RING_MAX = 4; 242 | int RING_IDS = 0; 243 | typedef struct { 244 | int *buff; 245 | int head; 246 | char *text; 247 | int color; 248 | } _RING; 249 | _RING RING[RING_MAX]; 250 | 251 | // data storage 252 | const int DATA_MSEC = 100; // sampling cycle in msec 253 | const int DATA_SIZE = 8*60*(1000/DATA_MSEC); // sampling storage size 254 | // 255 | int DATA_Setpoint[DATA_SIZE]; 256 | int DATA_Output[DATA_SIZE]; 257 | int DATA_Input[DATA_SIZE]; 258 | 259 | // data operations 260 | int data_init(int *buf, char *txt, int col=TFT_WHITE) { 261 | int id = RING_IDS; 262 | if (id < RING_MAX) { 263 | RING[id].buff = buf; 264 | RING[id].head = 0; 265 | RING[id].text = txt; 266 | RING[id].color = col; 267 | for (int i=0; i0? min(N,lastData): N); 283 | 284 | for (int id=0; idprint("SEC,"); 321 | for (int id=0; idprint(RING[id].text); 323 | cl->print(idprintf("%.3f,", n*DATA_MSEC/1000.0); 328 | for (int id=0; idprint(A[p]); 331 | cl->print(id0? min(N,lastData): N); 350 | float MAE = 0.0; 351 | p = (p-LAST+N)%N; 352 | for (int n=0; n0? min(N,lastData): N); 364 | float MSE = 0.0; 365 | p = (p-LAST+N)%N; 366 | for (int n=0; nprint(KEYS[n]); 442 | cl->print(nprint(CONFIG[n]); 447 | cl->print(n 480 | 481 | 482 | 483 | %s 484 | 485 | 486 |
487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 |
namerangevaluedescription
KG0IMU gain G (0-100)
KP0PID gain P (0-100)
KI0PID gain I (0-100)
KD0PID gain D (0-100)
CH100:NOR, 1:REV
CH300:TB, 1:KG, 2:KP, 3:KI, 4:KD, 5:NO
PWM50PWM frequency (Hz)
497 | 498 | 499 | 500 | 501 |
502 | 503 | 519 | 520 | )"; 521 | 522 | // HTML buffer 523 | char HTML_BUFFER[sizeof(HTML_TEMPLATE)+sizeof(WIFI_SSID)+8*SIZE]; 524 | 525 | // foward prototype 526 | void gpid_init(bool); 527 | 528 | // Web server for config 529 | bool configAccepted = false; 530 | void serverLoop() { 531 | WiFiClient client = WIFI_SERVER.available(); 532 | 533 | if (client) { 534 | //Serial.println("New Client."); 535 | String currentLine = ""; 536 | 537 | while (client.connected()) { 538 | if (client.available()) { 539 | char c = client.read(); 540 | //Serial.write(c); 541 | if (c == '\n') { 542 | if (currentLine.length() == 0) { 543 | // response for request "/" 544 | client.println("HTTP/1.1 200 OK"); 545 | client.println("Content-type:text/html; charset=utf-8;"); 546 | client.println(""); 547 | sprintf(HTML_BUFFER,HTML_TEMPLATE, WIFI_SSID,CONFIG[_KG],CONFIG[_KP],CONFIG[_KI],CONFIG[_KD],CONFIG[_CH1],CONFIG[_CH3],CONFIG[_PWM],0); 548 | client.println(HTML_BUFFER); 549 | break; 550 | } 551 | else 552 | if (currentLine.indexOf("GET /?") == 0) { 553 | // response for request "/?KG=..." 554 | int p1 = 0; 555 | int p2 = 0; 556 | int val = 0; 557 | // set CONFIG 558 | for (int n=0; n<(SIZE-TAIL); n++) { 559 | char key[16]; 560 | sprintf(key,"%s=",KEYS[n]); 561 | p1 = currentLine.indexOf(key, p2) + strlen(key); 562 | p2 = currentLine.indexOf('&', p1); 563 | val = currentLine.substring(p1, p2).toInt(); 564 | CONFIG[n] = val; 565 | } 566 | // set RTC 567 | p1 = currentLine.indexOf("JST=",p2) + strlen("JST="); 568 | RTC_DATE.Year = currentLine.substring(p1+0,p1+4).toInt(); 569 | RTC_DATE.Month = currentLine.substring(p1+4,p1+6).toInt(); 570 | RTC_DATE.Date = currentLine.substring(p1+6,p1+8).toInt(); 571 | M5.Rtc.SetData(&RTC_DATE); 572 | RTC_TIME.Hours = currentLine.substring(p1+8,p1+10).toInt(); 573 | RTC_TIME.Minutes = currentLine.substring(p1+10,p1+12).toInt(); 574 | RTC_TIME.Seconds = currentLine.substring(p1+12,p1+14).toInt(); 575 | M5.Rtc.SetTime(&RTC_TIME); 576 | // save CONFIG 577 | config_puts(); 578 | ch1_setFreq(CONFIG[_PWM]); 579 | gpid_init(true); 580 | // response 581 | client.println("HTTP/1.1 200 OK"); 582 | client.println("Content-type:text/html; charset=utf-8;"); 583 | client.println(""); 584 | sprintf(HTML_BUFFER,HTML_TEMPLATE, WIFI_SSID,CONFIG[_KG],CONFIG[_KP],CONFIG[_KI],CONFIG[_KD],CONFIG[_CH1],CONFIG[_CH3],CONFIG[_PWM],1); 585 | client.println(HTML_BUFFER); 586 | configAccepted = true; 587 | break; 588 | } 589 | else 590 | if (currentLine.indexOf("GET /csv") == 0) { 591 | // response for request "/csv" 592 | client.println("HTTP/1.1 200 OK"); 593 | client.println("Content-type:text/csv; charset=utf-8;"); 594 | //client.println("Content-Disposition:attachment; filename=data.csv"); 595 | client.printf("Content-Disposition:attachment; filename=data-%04d%02d%02d-%02d%02d.csv\n", RTC_DATE.Year,RTC_DATE.Month,RTC_DATE.Date, RTC_TIME.Hours,RTC_TIME.Minutes); 596 | client.println(""); 597 | config_dump(&client); 598 | data_dump(&client); 599 | client.println(""); 600 | break; 601 | } 602 | else 603 | { 604 | currentLine = ""; 605 | } 606 | } else if (c != '\r') { 607 | currentLine += c; 608 | } 609 | } 610 | } 611 | client.stop(); 612 | //Serial.println("Client Disconnected."); 613 | } 614 | } 615 | 616 | void setup_by_wifi() { 617 | // 618 | //WIFI_SERVER.begin(); 619 | // 620 | if (canvas_header("WIFI",0)) { 621 | char url[32]; 622 | canvas.println("[A] HOME"); 623 | canvas.println("SSID:"); canvas.printf(" %s\n",WIFI_SSID); 624 | canvas.println("PASS:"); canvas.printf(" %s\n",(WIFI_PASS==NULL? "": WIFI_PASS)); 625 | canvas.println("IP:"); canvas.print(" "); canvas.println(WIFI_IP); 626 | canvas_footer("WIFI"); 627 | sprintf(url,"http://%d.%d.%d.%d/",((WIFI_IP>>0)&0xff),((WIFI_IP>>8)&0xff),((WIFI_IP>>16)&0xff),((WIFI_IP>>24)&0xff)); 628 | M5.Lcd.qrcode(url,0,120,120,2); 629 | } 630 | delay(GUI_MSEC); 631 | // 632 | configAccepted = false; 633 | while (!configAccepted) { 634 | serverLoop(); 635 | vin_watch(); 636 | M5.update(); 637 | if (M5.BtnA.isPressed()) { 638 | delay(GUI_MSEC); 639 | return; 640 | } 641 | //delay(2); 642 | } 643 | // 644 | //WIFI_SERVER.end(); 645 | // 646 | } 647 | 648 | 649 | // config for ch1 end points 650 | void setup_ch1ends() { 651 | int ch1,val; 652 | for (int n=0; n<2; n++) { 653 | delay(GUI_MSEC); 654 | while (true) { 655 | ch1 = pulseIn(CH1_IN,HIGH,PWM_WAIT); 656 | val = map(ch1, 0,PWM_USEC, 0,PWM_DUTY); 657 | //ledcWrite(PWM_CH1,(ch1>0? val: 0)); 658 | ch1_setUsec(ch1); 659 | if (canvas_header("ENDS",LCD_MSEC)) { 660 | canvas.println((n? "LEFT": "RIGHT")); 661 | canvas.printf("[A] SAVE\n"); 662 | canvas.printf("[B] HOME\n"); 663 | canvas.printf(" CH1:%6d\n",ch1); 664 | canvas.printf(" VAL:%6d\n",val); 665 | canvas.printf(" |%s| \n",(n? "<<<< ": " >>>>")); 666 | canvas_footer("ENDS"); 667 | } 668 | M5.update(); 669 | if (M5.BtnA.isPressed()) { 670 | if (ch1>0) CONFIG[(n? _MAX: _MIN)] = ch1; 671 | break; 672 | } 673 | else 674 | if (M5.BtnB.isPressed()) { 675 | //ch1_setUsec(0); 676 | delay(GUI_MSEC); 677 | return; 678 | } 679 | } 680 | } 681 | // save 682 | if (ch1>0) { 683 | if (CONFIG[_MAX] < CONFIG[_MIN]) { 684 | int temp = CONFIG[_MAX]; 685 | CONFIG[_MAX] = CONFIG[_MIN]; 686 | CONFIG[_MIN] = temp; 687 | } 688 | config_puts(); 689 | } 690 | //ch1_setUsec(0); 691 | delay(GUI_MSEC); 692 | } 693 | 694 | 695 | 696 | ////////////////////////////////////////////////// 697 | // Low/High Pass Filters 698 | ////////////////////////////////////////////////// 699 | //float LPF_ME[4]; 700 | //float LPF_MAE[4]; 701 | //float LPF_MSE[4]; 702 | //// LPF 703 | //void lpf_init(float buf[],float alpha) { 704 | // buf[0] = 0.; 705 | // buf[1] = alpha; 706 | //} 707 | //float lpf_update(float buf[],float x) { 708 | // float yp = buf[0]; 709 | // float alpha = buf[1]; 710 | // float y = alpha*x + (1.-alpha)*yp; 711 | // buf[0] = y; 712 | // return y; 713 | //} 714 | //// HPF 715 | //void hpf_init(float buf[],float alpha) { 716 | // buf[0] = 0.; 717 | // buf[1] = 0.; 718 | // buf[2] = alpha; 719 | //} 720 | //float hpf_update(float buf[],float x) { 721 | // float yp = buf[0]; 722 | // float xp = buf[1]; 723 | // float alpha = buf[2]; 724 | // float y = alpha*(x - xp) + alpha*yp; 725 | // buf[0] = y; 726 | // buf[1] = x; 727 | // return y; 728 | //} 729 | 730 | 731 | 732 | ////////////////////////////////////////////////// 733 | // Initial calibrators 734 | ////////////////////////////////////////////////// 735 | float CH1US_MEAN; 736 | float OMEGA_MEAN[3]; 737 | float ACCEL_MEAN[3]; 738 | 739 | // Frequency counter 740 | int countHz(bool getOnly = false) { 741 | static unsigned long lastTime = 0; 742 | static int count = 0; 743 | static int freq = 50; 744 | if (!getOnly) count = count + 1; 745 | if (lastTime + 1000 < millis()) { 746 | freq = count; 747 | count = 0; 748 | lastTime = millis(); 749 | } 750 | return freq; 751 | } 752 | 753 | void mean_init(void) { 754 | unsigned long startTime; 755 | int count; 756 | // wait 757 | while (pulseIn(CH1_IN,HIGH,PWM_WAIT)==0) { 758 | vin_watch(); 759 | if (canvas_header("WAIT", LCD_MSEC)) { 760 | for (int n=0; n0? CH1_USEC - CH1US_MEAN: 0.0; 906 | Input = Kg * yrate; 907 | GyroPID.Compute(); 908 | ch1_usec = constrain(CH1US_MEAN + Output, CONFIG[_MIN],CONFIG[_MAX]); 909 | 910 | // Output PWM 911 | ch1_setUsec((CH1_USEC>0? ch1_usec: 0)); 912 | } 913 | // 914 | bool gpid_timing(int usec) { 915 | static unsigned long lastTime = 0; 916 | if (lastTime + usec < micros()) { 917 | lastTime = micros(); 918 | return true; 919 | } 920 | return false; 921 | } 922 | 923 | 924 | 925 | ////////////////////////////////////////////////// 926 | // put your setup code here, to run once: 927 | ////////////////////////////////////////////////// 928 | void setup() { 929 | // (1) setup M5StickC object 930 | M5.begin(); 931 | M5.IMU.Init(); 932 | //while (!setCpuFrequencyMhz(80)); 933 | 934 | // (2) setup configuration 935 | canvas_init(); 936 | config_init(); 937 | 938 | // (3) setup WiFi AP 939 | wifi_init(); 940 | 941 | // (4) setup GPIO 942 | pinMode(CH1_IN,INPUT); 943 | pinMode(CH3_IN,INPUT); 944 | pinMode(CH1_OUT,OUTPUT); 945 | pwmin_init(CH1_IN,&CH1_USEC,&CH1_FREQ,PWM_WAIT); 946 | pwmin_init(CH3_IN,&CH3_USEC,&CH3_FREQ,PWM_WAIT); 947 | ch1_setFreq(CONFIG[_PWM]); 948 | gpio25_dis_init(); 949 | 950 | // (5) Initialize Ring buffer 951 | data_init(DATA_Setpoint,"CH1",TFT_CYAN); 952 | data_init(DATA_Output,"SRV",TFT_MAGENTA); 953 | data_init(DATA_Input,"YAW",TFT_YELLOW); 954 | 955 | // (6) setup Zeros/Means 956 | mean_init(); 957 | 958 | // (7) setup PID 959 | gpid_init(true); 960 | 961 | // (8) setup others 962 | //Serial.begin(115200); 963 | } 964 | 965 | 966 | 967 | ////////////////////////////////////////////////// 968 | // put your main code here, to run repeatedly: 969 | ////////////////////////////////////////////////// 970 | void loop() { 971 | // Fetch Setpoint/Input and compute Output by PID 972 | //CH1_USEC = pulseIn(CH1_IN,HIGH,PWM_WAIT); 973 | if (gpid_timing(PWM_USEC)) { 974 | gpid_update(); 975 | countHz(); 976 | } 977 | 978 | // Sample PID variables in every 100msec 979 | if (data_sample(DATA_MSEC)){ 980 | data_put(0,Setpoint); 981 | data_put(1,Output); 982 | data_put(2,Input); 983 | } 984 | 985 | // Monitor variables in every 500msec 986 | if (canvas_header("HOME",LCD_MSEC)) { 987 | int lastData = 8*1000/DATA_MSEC; 988 | int lastLine = 1; 989 | int ch3_gain; 990 | // RCV monitor 991 | //canvas.println("RCV (us)"); lastLine++; 992 | //canvas.printf( " CH1:%6d\n", CH1_USEC); lastLine++; 993 | //canvas.printf( " CH2:%6d\n", CH2_USEC); lastLine++; 994 | //canvas.printf( " CH3:%6d\n", CH3_USEC); lastLine++; 995 | // PWM monitor 996 | canvas.println("PWM (Hz)"); lastLine++; 997 | canvas.printf( " IN :%6d\n", CH1_FREQ); lastLine++; 998 | canvas.printf( " OUT:%6d\n", PWM_FREQ); lastLine++; 999 | canvas.printf( " PID:%6d\n", countHz(true)); lastLine++; 1000 | // IMU monitor 1001 | //canvas.println("OMEGA (rad/s)"); lastLine++; 1002 | //canvas.printf( " X:%8.2f\n", IMU_OMEGA[0]); lastLine++; 1003 | //canvas.printf( " Y:%8.2f\n", IMU_OMEGA[1]); lastLine++; 1004 | //canvas.printf( " Z:%8.2f\n", IMU_OMEGA[2]); lastLine++; 1005 | //canvas.println("ACCEL (G)"); lastLine++; 1006 | //canvas.printf( " X:%8.2f\n", IMU_ACCEL[0]); lastLine++; 1007 | //canvas.printf( " Y:%8.2f\n", IMU_ACCEL[1]); lastLine++; 1008 | //canvas.printf( " Z:%8.2f\n", IMU_ACCEL[2]); lastLine++; 1009 | // PID monitor 1010 | canvas.println("PID (0-100)"); lastLine++; 1011 | canvas.printf( " G/P:%3d/%3d\n", CONFIG[_KG],CONFIG[_KP]); lastLine++; 1012 | canvas.printf( " I/D:%3d/%3d\n", CONFIG[_KI],CONFIG[_KD]); lastLine++; 1013 | //canvas.printf( " MAE:%6.1f\n", data_MAE(0,2,lastData)); lastLine++; 1014 | //canvas.printf( "RMSE:%6.1f\n", data_RMSE(0,2,lastData)); lastLine++; 1015 | // RGB graph 1016 | data_grid(0); 1017 | data_grid(CONFIG[_MIN]-CH1US_MEAN); 1018 | data_grid(CONFIG[_MAX]-CH1US_MEAN); 1019 | data_draw(lastData,lastLine); 1020 | // LCD draw 1021 | canvas_footer("HOME"); 1022 | 1023 | // CH3 >> CONFIG 1024 | //CH3_USEC = pulseIn(CH3_IN,HIGH,PWM_WAIT); 1025 | if (CH3_USEC > 0) { 1026 | ch3_gain = map(CH3_USEC, PULSE_MIN,PULSE_MAX, -GAIN_AMP,GAIN_AMP); 1027 | ch3_gain = constrain(ch3_gain, GAIN_MIN,GAIN_MAX); 1028 | switch (CONFIG[_CH3]) { 1029 | case 1: CONFIG[_KG] = ch3_gain; break; 1030 | case 2: CONFIG[_KP] = ch3_gain; break; 1031 | case 3: CONFIG[_KI] = ch3_gain; break; 1032 | case 4: CONFIG[_KD] = ch3_gain; break; 1033 | case 5: CONFIG[_KG] = 50; CONFIG[_KG] = CONFIG[_KI] = CONFIG[_KD] = 0; break; 1034 | default: break; 1035 | } 1036 | } 1037 | // CONFIG >> QuickPID 1038 | gpid_init(); 1039 | } 1040 | 1041 | // Watch vin and buttons 1042 | vin_watch(); 1043 | M5.update(); 1044 | if (M5.BtnA.isPressed()) setup_by_wifi(); 1045 | else 1046 | if (M5.BtnB.isPressed()) setup_ch1ends(); 1047 | } 1048 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 hshin-git 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README-en.md: -------------------------------------------------------------------------------- 1 | [日本語](README.md) | [English](README-en.md) 2 | 3 | # GyroM5 4 | 5 | - GyroM5 is an OSS for turning your [M5AtomMatrix](https://github.com/m5stack/M5Atom) / [M5StickC](https://github.com/m5stack/M5StickC) into steering assit gyro of RC drift car. 6 | - Sketch [GyroM5Atom.ino](GyroM5Atom/GyroM5Atom.ino) installed [M5AtomMatrix](https://github.com/m5stack/M5Atom) can stabilize drift driving of your RC car. 7 | - Sketch [GyroM5Stick.ino](GyroM5Stick/GyroM5Stick.ino) installed [M5StickC](https://github.com/m5stack/M5StickC) can stabilize drift driving of your RC car. 8 | 9 | 10 | ![GyroM5](https://user-images.githubusercontent.com/64751855/117384511-1d46f000-af1e-11eb-854e-45ee149e4671.jpg) 11 | 12 | 13 | --- 14 | 15 | 16 | # DEMO 17 | This Tamiya RC car (SU-01) with GyroM5 is performing "RWD drifting". 18 | 19 | https://user-images.githubusercontent.com/64751855/117535983-a1d76280-b033-11eb-9f59-ec6aaef0b9b0.mp4 20 | 21 | 24 | 25 | 26 | 27 | # Features 28 | GyroM5 has unique features. 29 | 30 | - Feedback control
Auto steering to follow target yaw rate by PID control 31 | - Parameter setting
Setting PID control parameters by smartphone 32 | - Remote gain tuning
Tuning a PID control parameter by CH3 signal 33 | - End point setting
Setting steering end point by CH1 signal 34 | - IMU calibration
Auto calibration of zero points in CH1 and IMU 35 | - Monitoring display
Displaying RC/IMU signals and PID parameters in LCD 36 | - Drift detection
Detecting counter-steer and appearing by LCD 37 | 38 | 39 | # Requirement 40 | These hardwares are required for GyroM5. 41 | 42 | - Hobby RC car　
RC car equipped with standard and separated receiver/servo units. 43 | - Standard PC
PC installed with Arduino IDE and equipped with USB. 44 | - M5StickC
"M5StickC" not "M5StickC Plus" is required. 45 | - Parts for wire harness
One servo extention cable, and one pin headder (8-pin male). 46 | - Soldering tool
For assembling wire harness. 47 | 48 | 49 | 50 | # Usage 51 | The outline of usage is as follows. The details are in next section. 52 | 53 | ## Hardware setting 54 | 1. Install Arduino IDE on your PC. 55 | 2. Setup Arduino IDE for ESP/M5StickC. 56 | 3. Connect your PC and M5StickC with USB. 57 | 4. Install sketch [GyroM5Stick.ino](GyroM5Stick/GyroM5Stick.ino) on your M5StickC. 58 | 5. Install GyroM5/M5StickC on your RC car with LCD up. 59 | 60 | ## Software setting 61 | 1. Turn on your RC car. 62 | 2. Wait for GyroM5 HOME state. 63 | 3. Setup steering end point. 64 | 4. Setup initial PID control gains (KG=50, KP=50, KI=30, KD=10). 65 | 5. Run RC car and adjust PID control gains. 66 | 67 | ## Daily usage 68 | 1. Turn on your RC car. 69 | 2. Wait for GyroM5 HOME state. 70 | 3. Run RC car and adjust PID control gains. 71 | 72 | 73 | --- 74 | # Note 75 | The details are as follows. 76 | 77 | ## Wiring 78 | Wire GyroM5/M5StickC to RC receiver/servo units, as explained in the table below. 79 | 80 | ![GyroM5-hardware](https://user-images.githubusercontent.com/64751855/128596160-57cea8d3-d4de-4b73-8d0f-7e85df9dcbab.png) 81 | 82 | |M5StickC |in/out |RC units | 83 | |---- |---- |---- | 84 | |G26 |in | Reciever CH1| 85 | |G36 |in | Reciever CH3| 86 | |G0 |out | Servo CH1| 87 | |GND |in | Reciever minus| 88 | |5Vin |in | Reciever plus| 89 | 90 | An example image of assembled wire harness is as follows. 91 | 92 | ![GyroM5-wireharness](https://user-images.githubusercontent.com/64751855/128596101-5880e0f9-746c-4c2b-a70c-1ee10ea8078b.png) 93 | 94 | Caution: 95 | Signal levels in M5StickC (3.3v) and RC units (5.0v or more) are generally different. 96 | My RC units use 5.0-6.0v and work no trouble with directly connected M5StickC. 97 | But higher volotage (over 6.0v) RC units may damage your M5StickC. 98 | 99 | ![GyroInstall](https://user-images.githubusercontent.com/64751855/117384355-b75a6880-af1d-11eb-88ad-850f1de2ef77.jpg) 100 | 101 | 102 | ## Turning On/off 103 | M5Stick is known to have bugs in its power managment. 104 | Find hints for trouble-shooting with google search like keyword "m5stickc not turning on". 105 | 106 | 107 | 108 | ## Monitoring LCD 109 | GyroM5 has five states below. 110 | One state transits to anothr state at button [A]/[B] or timeout event. 111 | 112 | ![GyroM5-state](https://user-images.githubusercontent.com/64751855/128596141-f6c28196-3827-4584-86fa-db1593254b71.png) 113 | 114 | - State "HOME" is the home, transits to "WIFI" by [A] and transits to "ENDS" by [B]. 115 | - State other than "HOME" accepts A/B button or returns to "HOME" by timeout. 116 | - Remote gain tuning by CH3 is initially disabled. 117 | 118 | |state|transition|descripition| 119 | |----|----|----| 120 | |WAIT |RC signal |waits for PWM signal from RC receiver| 121 | |INIT |Timeout |calibrates zero points in CH1 and gyrosensor, dont move RC car| 122 | |HOME |[A],[B] |displays RC signals, IMU inputs and PID gains| 123 | |WIFI |[A] |WiFi access point and accepts setting commands| 124 | |ENDS |[B] |setup CH1 steering end points| 125 | 126 | 127 | GyroM5's web server returns the following page for various setting. 128 | In this page, you can setup PID parameters, PWM frequency and so on. 129 | 130 | ![GyroM5-wifi-link](https://user-images.githubusercontent.com/64751855/128596121-7f20ad39-d4e7-4f01-a30e-5d913585112c.png) 131 | 132 | 133 | ## Tuning 134 | GyroM5's control algorithm is explained for tuning parameters. 135 | 136 | 137 | ### Algorithm 138 | GyroM5 uses generic feedback control algorithm "PID control". 139 | 140 | ![PID_wikipedia](https://upload.wikimedia.org/wikipedia/commons/thumb/4/43/PID_en.svg/800px-PID_en.svg.png) 141 | 142 | In the above PID control, the plant/process is your RC car. 143 | The target r, the output y and the control u are as follows. 144 | 145 | - target: r = ch1_in = CH1 input from RC receiver 146 | - output: y = Kg*wz = Yaw rate of RC car 147 | - control: u = ch1_out = CH1 output to RC servo 148 | 149 | GyroM5 attempts to minimize error value e by adjusting control variable u. 150 | 151 | - error: e = r - y = ch1_in - Kg*wz 152 | - control: u = PID(e) = Kp * e + Ki * INT(e) + Kd * DOT(e) 153 | 154 | INT is time integral operator, DOT is time derivative operator. 155 | 156 | 157 | ### Parameters 158 | You can confirm/adjust the integer PID gains by LCD, A/B buttons and CH1. 159 | The integer gains (in uppercase) are normalized from -100 to 100, and are related to the real gains (in lowercase) as follows. 160 | 161 | - Yaw rate "wz" is in (radian per sec):
IMU sensored values in physical units. 162 | - Input/output "ch1" is in 16bits (0〜64k):
Pulse width (0〜20ms=1000ms/50Hz) in 16bit (0〜2^16-1) integer. 163 | - Observation Gain: Kg = KG/20.0
Larger Kg becomes, smaller yaw rate per steering becomes. 164 | - Proportional Gain: Kp = KP/50.0
Larger Kp becomes, more fastly error decreases but may vibrate. 165 | - Integral Gain: Ki = KI/250.0
Larger Ki becomes, more slowly error decreases, and smaller final error is. 166 | - Derivative Gain: Kd = KD/5000.0
Larger Kd becomes, more quickly error decreases but may vibrate. 167 | 168 | Initial parameters are recommended to set the integer gains "KG=50, KP=60, KI=30, KD=10". 169 | Special integer gains "KG=KI=KD=0 and KP=50" are as same as the setting "pass throw: u=r". 170 | The plus/minus sign of KG is used for normal/reverse operation in steering servo. 171 | 172 | 173 | ## Testing 174 | My RC car for testing GyroM5 is as follows. 175 | 176 | ![UpperView](https://user-images.githubusercontent.com/64751855/117554986-370b4300-b096-11eb-9ef8-50a00980d9fc.jpg) 177 | 178 | |item |model | 179 | |----|----| 180 | |chassis |Tamiya SU-01| 181 | |body |Tamiya Jimmny Willy (SJ30) | 182 | |tire |TOPLINE drift tire| 183 | |RC TX |Tamiya fine spec 2.4GHz| 184 | |RC RX |Tamiya TRE-01| 185 | |RC ESC |Tamiya TRE-01| 186 | |RC servo |Yokomo S-007| 187 | |battery |7.4V LiPo 1100mAh| 188 | |motor | 370 type| 189 | 190 | Hints for "RWD drifting" are listed bellow. 191 | 192 | ![LowerView](https://user-images.githubusercontent.com/64751855/117554999-51ddb780-b096-11eb-81c1-7907ea12db07.jpg) 193 | 194 | - Larger steering angle is better for controlability. 195 | - Faster steering servo is also better. 196 | - Slippy tires are easier to drift by low power motor/battery. 197 | 198 | 199 | 200 | # Roadmap 201 | The followings are some ideads for improving your GyroM5. 202 | 203 | - Wireless setting GyroM5 by smartphone 204 | - Automatic tuning of PID gain parameters 205 | - Assiting not only steering but also throttle 206 | - Reading PWM input without blocking 207 | - Automatic detection of installed direction 208 | - Recording and analysis of driving data 209 | 210 | 211 | # Author 212 | The author bought a small RC car kit (Tamiya SU-01) for indoor playing under COVID-19. 213 | After purchasing, I watched the RC car YouTube channel and became interested in "RC drift car" that did not exist in my childhood. 214 | The "RC drift car" is already established as a genre of RC car, 215 | and the shortest course to play "RC drift car" is to get dedicated products like Yokomo YD-2. 216 | 217 | But in my case, 218 | I noticed the "RC drift car" after purchasing the kit, 219 | and I believed that any RC car can perform "stable drift driving" by high speed control. 220 | So I tried to make steering assit gyro to stabilize RWD drift driving for my small car. 221 | 222 | I enjoyed making this GyroM5, 223 | and I thought this may be a good material to learn programming and control algorithm while playing RC car. 224 | So I release the source code of GyroM5 for expecting that someone use this material for STEM education, 225 | or persuading your parents to buy hobby RC car. 226 | 227 | I am happy if somebody could reproduce GyroM5 or customize it by themselves. 228 | 229 | (^_^) 230 | 231 | --- 232 | 233 | # Reference 234 | 235 | 236 | ## Radio Control Car 237 | - [Yokomo YD-2](https://teamyokomo.com/product/dp-yd2/) 238 | - [Tamiya SU-01](https://www.tamiya.com/japan/products/product_info_ex.html?genre_item=7101) 239 | - [RC Car Shop "Genkikko-san"](https://genkikkosan.com/) 240 | 241 | ## Automobile Drifting 242 | - [Automotive Vehicle Dynamics](https://www.amazon.co.jp/dp/4501419202/) 243 | - [Vehicle Running Stability Analysis and Spin Control](https://www.tytlabs.com/japanese/review/rev321pdf/321_013ono.pdf) 244 | - [On the dynamics of automobile drifting](https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.103.9227&rep=rep1&type=pdf) 245 | - [Analysis and control of high sideslip manoeuvres](https://www.tandfonline.com/doi/abs/10.1080/00423111003746140?journalCode=nvsd20) 246 | - [Stabilization of steady-state drifting for a RWD vehicle](http://dcsl.gatech.edu/papers/avec10.pdf) 247 | 248 | ## Software 249 | - [M5StickC Library](https://github.com/m5stack/M5StickC) 250 | - [M5StickC non official reference](https://lang-ship.com/reference/unofficial/M5StickC/) 251 | - [M5Stack official documents](https://github.com/m5stack/m5-docs/blob/master/docs/ja/README_ja.md) 252 | - [Arduino IDE](https://www.arduino.cc/en/software) 253 | - [PID Controller - Wikipedia](https://en.wikipedia.org/wiki/PID_controller) 254 | 255 | ## Hardware 256 | - [M5StickC device](https://www.switch-science.com/catalog/5517/) 257 | - [pin header (male)](https://www.amazon.co.jp/dp/B012HY288S/) 258 | - [servo extention cable](https://www.amazon.co.jp/dp/B00W9ST610/) 259 | 260 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [日本語](README.md) | [English](README-en.md) 2 | 3 | # GyroM5 4 | 5 | - GyroM5は、ドリフトRCカーのステアリングジャイロを開発ボード[M5AtomMatrix](https://github.com/m5stack/M5Atom) / [M5StickC](https://github.com/m5stack/M5StickC)で自作するためのオープンソースソフトウェアです。 6 | - 本格的なPID制御アルゴリズムの採用により、ステアリングのアシスト機能を高い自由度で設定できます。 7 | - [M5AtomMatrix](https://github.com/m5stack/M5Atom)にファームウェア[GyroM5Atom.ino](GyroM5Atom/GyroM5Atom.ino)をインストールしてRCユニットと接続すれば完成です。 8 | - [M5StickC](https://github.com/m5stack/M5StickC)にファームウェア[GyroM5Stick.ino](GyroM5Stick/GyroM5Stick.ino)をインストールしてRCユニットと接続すれば完成です。 9 | 10 | ![ラジコン画像](https://user-images.githubusercontent.com/64751855/117384511-1d46f000-af1e-11eb-854e-45ee149e4671.jpg) 11 | 12 | 13 | --- 14 | 15 | 16 | # DEMO 17 | GyroM5搭載のタミヤ製RCカー（SU-01シャーシ）がRWDドリフト走行するデモ動画です。 18 | 19 | 22 | 23 | https://user-images.githubusercontent.com/64751855/117535983-a1d76280-b033-11eb-9f59-ec6aaef0b9b0.mp4 24 | 25 | 26 | # Features 27 | 28 | GyroM5（v1/v2ファームウェア）は、以下のユニークな特徴を備えています。 29 | PID制御は、汎用的なフィードバック制御アルゴリズムであり、パラメータ4個の調整により自由度の高いセッティングが可能です。 30 | 31 | - フィードバック制御機能
車体ヨーレートを目標値に近付けるように操舵角を自動制御（PID制御）します。 32 | - 制御パラメータ設定機能
フィードバック制御のパラメータをスマホから設定して保存できます。 33 | - リモートゲイン調整機能
フィードバック制御のパラメータをCH3からリモート調整できます。 34 | - エンドポイント設定機能
ステアリング角度のエンドポイントを設定して保存できます。 35 | - スマートフォン連携機能（v2）
WiFi通信によりスマートフォン経由でジャイロ設定が可能です。 36 | - サーボ周波数可変機能（v2）
サーボ用のPWM周波数とPID制御周期を50Hz〜400Hzの範囲で変更できます。 37 | - 走行データ表示機能（v2）
PID制御データ（目標値、入出力）をLCD画面にグラフ表示できます。 38 | - 走行データ保存機能（v2）
WiFi通信によりPID制御データをCSVファイルでダウンロードできます。 39 | - 鉛直方向検出機能（v2）
起動時に鉛直方向を自動検出するので、ジャイロのシャーシ固定方向に制約がありません。 40 | - 時刻表示機能（v2）
現在時刻をLCD画面に表示できます。 41 | 42 | 43 | 44 | # Requirement 45 | GyroM5の利用に必要なハードウェアの要件を列挙します。 46 | 47 | - ホビー用RCカー　
標準的なRCユニットを搭載して、受信機とステアリング用サーボを3線（-+S）で接続するRCカーです。 48 | - 標準的パソコン
Arduino IDEをインストールでき、USBインターフェイスを備えるスケッチ書き込み用パソコンです。 49 | - M5StickC
LCD解像度の低い方が"M5StickC"です。高い方の"M5StickC Plus"ではありません。 50 | - ワイヤハーネス部品
サーボ延長ケーブルを1本、オス型ピンヘッダを1個（8ピン以上）使います。 51 | - ハンダ付け機材
ワイヤハーネスの組み立てに使います。 52 | 53 | M5StickCやワイヤハーネス部品は、Amazon等のネット通販で入手可能です。 54 | 55 | 56 | # Usage 57 | GyroM5の使い方（本体準備、初期設定から通常利用の流れ）を概説します。 58 | RCユニットとの接続方法、M5StickCの起動方法、制御パラメータの調整方法は後半を参照ください。 59 | 60 | ## 本体準備 61 | 1. 手持ちのパソコンにArduino IDE（開発環境）をインストールする 62 | 2. Arduino IDEの開発ボード設定をESP32/M5StickC向けに変更する 63 | 3. パソコンとM5StickC開発ボードをUSBケーブルで接続する 64 | 4. ファームウェア[GyroM5Stick.ino](GyroM5Stick/GyroM5Stick.ino)をArduino IDE経由でM5StickCへ書き込む 65 | 6. GyroM5（M5StickC）をRCカーに固定（LCD画面が上向き）してRCユニットと接続する 66 | 67 | ## 初期設定 68 | 1. RCユニットの電源を入れる 69 | 2. GyroM5のHOME画面が出るの水平状態、RC無操作で待つ 70 | 3. GyroM5の操舵エンドポイントを設定する 71 | 4. GyroM5のPID制御パラメータを設定する（初期値：KG=50、KP=50、KI=30、KD=10） 72 | 5. RCカーを走らせて、必要によりPID制御パラメータを微調整する 73 | 74 | ## 通常利用 75 | 1. RCユニットの電源を入れる 76 | 2. GyroM5のHOME画面が出るのを水平状態、RC無操作で待つ 77 | 3. RCカーを走らせて、必要によりPID制御パラメータを微調整する 78 | 79 | 80 | --- 81 | # Note 82 | GyroM5の詳しい解説を記します。 83 | 84 | ## 接続方法 85 | 86 | GyroM5利用時のM5StickCの入出力ピン（GPIO端子）とRCユニット（受信機、サーボ）端子との接続方法は下記の通りです。 87 | 88 | ![GyroM5-hardware](https://user-images.githubusercontent.com/64751855/128596160-57cea8d3-d4de-4b73-8d0f-7e85df9dcbab.png) 89 | 90 | 91 | |M5StickC |in/out |RCユニット | 92 | |---- |---- |---- | 93 | |G26 |in | RC受信機CH1のシグナル端子| 94 | |G36 |in | RC受信機CH3のシグナル端子| 95 | |G0 |out | RCサーボCH1のシグナル端子| 96 | |GND |in | RCアンプBECのマイナス端子| 97 | |5Vin |in | RCアンプBECのプラス端子| 98 | 99 | ワイヤハーネス（接続ケーブル）は、CH1入出力用にRCサーボ用のコネクタ付き延長ケーブル1本を中央で切断して、8ピンヘッダ（オス）とハンダ付けすれば完成です。 100 | 101 | ![GyroM5-wireharness](https://user-images.githubusercontent.com/64751855/128596101-5880e0f9-746c-4c2b-a70c-1ee10ea8078b.png) 102 | 103 | ゲイン調整用にCH3入力を利用する場合、信号線（単線）のみ受信機CH3とG32を接続すれば機能します。 104 | なおCH3をジャイロに接続しない場合、ジャイロはPID制御の静的なパラメータ表のみ参照します。 105 | 106 | ![ジャイロ搭載](https://user-images.githubusercontent.com/64751855/117384355-b75a6880-af1d-11eb-88ad-850f1de2ef77.jpg) 107 | 108 | 信号の電圧レベルに関しては、M5StickC側が3.3Vなのに対して、RCユニット側が通常5.0V以上と高くなる点に十分ご注意ください。 109 | テスト環境（タミヤ製TRE-01、HobbyWing製QuicRUN-1060）では、直結で問題なく動いていますが、許容範囲内でも保証範囲外と思います。 110 | たとえばRCユニット側の電圧レベルが6Vを超える場合、レベルコンバータを省略するとM5StickCが破損（不可逆に故障）する恐れがあります。 111 | 112 | M5StickCのGPIO端子は、プログラムにより自由に変更できますが、G0端子を入力用に使うことは避けたほうが良いです。 113 | G0端子は、内部的にプルアップされており、電源投入時にG0端子がLowレベルだとM5StickCが起動しないことがありました。 114 | G0端子が出力用の場合、相手側の端子が入力用のハイインピーダンスとなるので、この問題を回避できるようです。 115 | 116 | 117 | 118 | ## 起動方法 119 | 120 | GyroM5は、RCユニットの電源と連動して起動、停止します。 121 | 122 | ただしM5StickCは、電源管理に不具合が残っており、素直に起動しない場合があります。 123 | 原因は、電源管理チップAXPの不具合、内蔵バッテリの電圧低下、GPIOピンの電圧レベル等に起因するそうです。 124 | キーワード「M5StickC　起動しない」のGoogle検索で、複数の対処方法が見つかりますので、トラブル解決の参考にしてください。 125 | 126 | 自分のテスト環境でも、起動に失敗する場合が起きましたが、M5StickCのワイヤハーネスを取り外してUSB給電すると高確率で起動します。 127 | M5StickC関連ブログ記事に「G0と3V3を直結してUSB給電すれば起動」や「電源投入時のG0電圧に応じてリセット動作」等の記載があります。 128 | 内蔵バッテリの充電不足を除いた場合、電源投入時点のGPIOピン（おそらくはG0ピンの）電圧によりM5StickC起動の成否が決まるようです。 129 | 130 | なおジャイロ起動に成功後、電源オフ操作は不要です。 131 | GyroM5プログラムは、5Vin端子でBEC電圧の低下を検出すると電源をオフします。 132 | 133 | 134 | ## 画面遷移 135 | 136 | [GyroM5Stick](GyroM5Stick/GyroM5Stick.ino)は、5種類の画面状態を遷移します。状態遷移は、ボタン[A]、[B]操作及びタイムアウト時に発生します。 137 | 138 | ![GyroM5-state](https://user-images.githubusercontent.com/64751855/128596141-f6c28196-3827-4584-86fa-db1593254b71.png) 139 | 140 | - 「HOME」が、ホーム画面となり、この状態からボタン[A]で「WIFI」へ、ボタン[B]で「ENDS」へ遷移します。 141 | - 「WIFI」は、WiFiアクセスポイント兼WWWサーバとなり、スマートフォン等のWWWクライアントの要求に対応します。 142 | - 「ENDS」は、ステアリングCH1のエンドポイント（ステアリング用サーボ角度の上下限値）を設定します。 143 | - 他の状態の場合、ボタン操作が画面に表示してあるか、無操作のタイムアウトでホーム状態「HOME」へ戻ります。 144 | 145 | |画面|遷移|解説| 146 | |----|----|----| 147 | |WAIT|RC受信機の起動|ジャイロ起動後、RC受信機からPWM信号を受信するまで待機| 148 | |INIT|タイムアウト|CH1ニュートラル位置、IMUバイアスのサンプリング（この間、送信機操作や車体振動がNG）| 149 | |HOME|ボタン[A],[B]|PID制御状態（RCユニット入出力、PID制御グラフなど）の表示| 150 | |WIFI|ボタン[A]|WiFiアクセスポイント兼WWWサーバとなりスマートフォン等の要求に対応| 151 | |ENDS|ボタン[B]|CH1エンドポイントの設定| 152 | 153 | WiFi機能を利用する場合、GyroM5のボタン[A]を押してください。GyroM5は、WIFIモードに入るとWiFiアクセスポイント兼WWWサーバとして機能します。 154 | スマホからGyroM5のWiFiアクセスポイントへ接続後、LCD画面のIPアドレスまたはQRコードで指定されるURLを開くと以下の画面が表示されます。 155 | なおWiFi接続に成功後、画面表示に失敗する場合は「モバイルデータ通信を一時的にOFF」にしてください。 156 | モバイルデータ通信がONの場合、スマホによりインターセット接続が有効でないWiFi通信へルーティングしません。 157 | この画面から各種パラメータの設定機能、走行データのダウンロード機能を利用できます。 158 | パラメータの設定後、GyroM5は通常のPID制御モード「HOME」へ自動復帰します。 159 | 160 | ![GyroM5-wifi-link](https://user-images.githubusercontent.com/64751855/128596121-7f20ad39-d4e7-4f01-a30e-5d913585112c.png) 161 | 162 | 163 | ## 調整方法 164 | GyroM5チューニング時の参考情報として、制御アルゴリズムを解説します。 165 | 166 | ### 制御アルゴリズム 167 | GyroM5は、汎用的なフィードバック制御アルゴリズムのPID制御（下図はWikipediaから引用）を利用します。 168 | 169 | ![PID_wikipedia](https://upload.wikimedia.org/wikipedia/commons/thumb/4/43/PID_en.svg/800px-PID_en.svg.png) 170 | 171 | PID制御における目標値r、出力値yおよび操作量uとRCカー（Plant/Process）の関係は以下のとおりです。 172 | 173 | - 目標値: r = ch1_in = RC受信機からのCH1入力 174 | - 出力値: y = kg*wz = RCカーの車体ヨーレート 175 | - 操作量: u = ch1_out = RCサーボへのCH1出力 176 | 177 | つまりRC受信機（送信機）からのCH1入力rを車体ヨーレートyの目標値と解釈して、 178 | 両者の偏差eをゼロに近づけるフィードバック制御により、サーボへのCH1出力uを自動調整します。 179 | 180 | - 偏差: e = r - y = ch1_in - Kg*wz 181 | - 操作量: u = PID(e) = Kp * (e) + Ki * INT(e) + Kd * DOT(e) 182 | 183 | INTは積分演算子、DOTは微分演算子を意味します。 184 | 185 | フィードバック制御の結果、グリップ走行時はニュートラルステアに近い回頭性、ドリフト走行時はヨーレートの安定性を期待できます。 186 | 187 | 188 | ### 制御パラメータ 189 | PID制御のパラメータ（Kg、Kp、Ki、Kd）は、走行コンディションにより調整すべきであり、LCD画面で確認＆変更できます。 190 | PID制御の整数値（大文字）は、数値を0〜100に規格化しており、PID制御の実数値（小文字）との関係は以下の通りです。 191 | 192 | - 角速度"wz"は（ラジアン/秒）単位:
慣性センサ（IMU）計測値をセンサ感度に応じて物理量へ変換した数値です。 193 | - 入出力"ch1"は16ビット数（0〜64k）:
PWMパルス幅（0ms〜20ms=1000ms/50Hz）を示す16ビット数（0〜2^16-1）です。 194 | - 観測ゲイン: Kg = KG/20.0
大きくするとヨーレートに敏感となり、ステアリング量に対する目標ヨーレートは小さくなります。 195 | - 比例ゲイン: Kp = KP/50.0
大きくするとカウンタステア量が多くなりますが、大きすぎるとハンチングします。 196 | - 積分ゲイン: Ki = KI/250.0
大きくするとカウンタステア応答が遅くなりますが、最終的な偏差を減らせます。 197 | - 微分ゲイン: Kd = KD/5000.0
大きくするとカウンタステア応答が早くなりますが、大きすぎるとハンチングします。 198 | 199 | テスト用RCカーの場合、設定値「KG=50、KP=60、KI=30、KD=10」程度でドリフト走行できました。 200 | なお特別な設定値「KG=KI=KD=0、KP=50」は、制御なし「入力を出力へスルー：u=r」と同じです。 201 | 202 | 203 | ## 試験環境 204 | 205 | 作者のようにSU-01シャーシでRWDドリフト走行を試みる人は少ないと思いますが、 206 | 参考までにテスト用RCカー、ユニット及びジャイロ搭載例の写真と諸元を記します。 207 | 208 | ![シャーシ表側](https://user-images.githubusercontent.com/64751855/117554986-370b4300-b096-11eb-9ef8-50a00980d9fc.jpg) 209 | 210 | |項目 |型番 | 211 | |----|----| 212 | |シャーシ|タミヤ製SU-01| 213 | |ボディ|タミヤ製ジムニーウイリー（SJ30）| 214 | |タイヤ|TOPLINE製Mシャーシ用ドリフトタイヤ| 215 | |送信機|タミヤ製ファインスペック2.4GHz| 216 | |受信機|タミヤ製TRE-01| 217 | |アンプ|タミヤ製TRE-01| 218 | |サーボ|ヨコモ製S-007| 219 | |バッテリ|7.4V LiPo 1100mAh| 220 | |モータ|ノーマル370型DCモーター| 221 | 222 | ドリフト走行に関連する注意点を列挙します。 223 | 224 | ![シャーシ裏側](https://user-images.githubusercontent.com/64751855/117554999-51ddb780-b096-11eb-81c1-7907ea12db07.jpg) 225 | 226 | - シャーシに関しては、ステアリング用ナックルとシャーシの干渉部分を削りステアリング角度を45度ぐらいまで増やしました。 227 | - サーボに関しては、ファインスペック付属のTSU-03だと制御が遅れてハンチングしたので、ある程度の高速なサーボが必要です。 228 | - モータに関しては、ノーマルだとLiPoバッテリと組み合わせないと、スピードが出たときにトルク不足でドリフト移行が難しいです。 229 | - タイヤに関しては、駆動系が非力なので、なるべく滑りやすいタイヤが良いです。 230 | 231 | 232 | # Roadmap 233 | 234 | RCカー用ジャイロ自作を通して、気付いた改良アイデアなどを列挙します。 235 | いずれ対応したいと思いますが、趣味で開発しているので、いつ対応できるか分かりません。 236 | ご自身で改良にチャレンジすれる際の参考になれば幸いです。 237 | 238 | - パラメータ設定のスマホ対応（v2対応）　
スマホのGUI画面からジャイロ設定（PIDゲイン等）を複数管理して変更可能とする。 239 | - パラメータ調整の完全自動化
車体、路面やタイヤに応じたPIDゲインの最適化を強化学習などで完全自動化する。 240 | - スロットル制御のアシスト
ドリフト走行の安定化には、ステアリングとスロットルの同時制御が必要です。 241 | - 加速度センサの有効利用
ヨーレートと水平加速度から車体スリップ角を推定してトラクション制御を高度化する。 242 | - ジャイロ固定方向の自動検出（v2対応）
鉛直方向を起動時に自動検出して車体ヨーレート成分を決定する。 243 | - PWM入力方式の改良（v2対応）
PWM入力にブロック方式の関数pulseIn(...)を廃止して割り込み方式へと変更する。 244 | - 外部電源との完全連動（v2対応）
M5StickCの内蔵バッテリーを無効化して、RCアンプBECの給電のみでオン/オフ動作させる。 245 | - 走行データの記録分析（v2対応）
走行データをSDカード等に記録して事後分析できるようにする（M5StickCからM5Stackへ変更？）。 246 | - サーボ周波数可変機能（v2対応）
サーボPWM周波数をパラメータで変更可能とする。 247 | 248 | M5StickCは、WiFi/Bluetoothを備える点、外部GPIOが5本ある点、6軸IMUを備える点、割り込み処理できる点から、 249 | ほとんどの改良案はハードウェア的には実現可能と思いますので、あとはソフトウェアつまりアイデア次第だと思います。 250 | 251 | 252 | # Author 253 | 254 | 作者は、コロナ禍で屋内遊びをさがす中、初代グラスホッパー（笑）以来めっちゃ久しぶりにRCカーキット（小型のタミヤSU-01シャーシ）を購入しました。 255 | 購入後、RCカー系YouTubeチャンネルを見て、子ども時代に存在しなかったドリフト用RCカー（通称、ドリラジ）の動きに興味を持ちました。 256 | ツルツルのタイヤで横滑りさせながらRCカーを走らせるアレです。 257 | ドリフト用RCカーは、ジャンルとして確立しており、たとえばヨコモYD-2のように専用設計で完成度の高い製品が存在します。 258 | 純粋にRCカーのドリフト走行を楽しみたければ、ドリフト専用のシャーシやジャイロ製品を入手するのが最短コースです。 259 | 260 | 自分の場合、ドリラジの存在に気付いたのがRCカーキット購入後だったこと、 261 | どんなRCカーでも上手く制御できればドリフト走行（を安定化）可能と信じていたことから、 262 | タミヤ最安（実売価格≒4K円）のSU-01シャーシを自作ジャイロで制御してドリフト走行にチャレンジしました。 263 | やや回り道しましたが、ほぼノーマル（舵角を増やしただけ）のSU-01シャーシでドリフト走行できました。 264 | 265 | RCカー用ジャイロの自作は、プログラムやパラメータの変更によりRCカーの走行特性を変えられるので楽しい開発でした。 266 | RCカー好きの人なら自作ジャイロの操縦性を楽しみつつ、プログラミングや制御アルゴリズムを習得する良い素材（STEM教育の素材）と思います。 267 | 趣味でRCカーやプログラミングを楽しむ若い人が増えて欲しいとの願いから、開発したRCカー用ジャイロGyroM5のソースコードを公開します。 268 | この記事を参考に、部品を集めてGyroM5を再現する人、改造して「オレ専用ジャイロ」を開発する人、が出てくれば自分はハッピーです。 269 | 270 | (^_^) 271 | 272 | 273 | --- 274 | 275 | # Reference 276 | 277 | RCカー用ジャイロGyroM5の開発にあたり、参考にした資料などを列挙します。 278 | 「元気っ子さん」は、初心者に親切なラジコン屋さんで、作者がGyroM5搭載カーの試験走行、ヨコモYD-2の体験走行等でお世話になっています。 279 | 280 | ## ホビー用RCカー関連 281 | - [ヨコモYD-2](https://teamyokomo.com/product/dp-yd2/) 282 | - [タミヤSU-01](https://www.tamiya.com/japan/products/product_info_ex.html?genre_item=7101) 283 | - [RCカー練習場「元気っ子さん」](https://genkikkosan.com/) 284 | 285 | ## ドリフト走行の理屈 286 | - [自動車の運動と制御](https://www.amazon.co.jp/dp/4501419202/) 287 | - [車両運動の安定性解析と制御への応用](https://www.tytlabs.com/japanese/review/rev321pdf/321_013ono.pdf) 288 | - [On the dynamics of automobile drifting](https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.103.9227&rep=rep1&type=pdf) 289 | - [Analysis and control of high sideslip manoeuvres](https://www.tandfonline.com/doi/abs/10.1080/00423111003746140?journalCode=nvsd20) 290 | - [Stabilization of steady-state drifting for a RWD vehicle](http://dcsl.gatech.edu/papers/avec10.pdf) 291 | 292 | ## ソフトウェア関連 293 | - [M5StickC Library](https://github.com/m5stack/M5StickC) 294 | - [M5StickC非公式日本語リファレンス](https://lang-ship.com/reference/unofficial/M5StickC/) 295 | - [M5Stack公式ドキュメント](https://github.com/m5stack/m5-docs/blob/master/docs/ja/README_ja.md) 296 | - [Arduino IDE](https://www.arduino.cc/en/software) 297 | - [PID Controller - Wikipedia](https://en.wikipedia.org/wiki/PID_controller) 298 | 299 | ## ハードウェア関連 300 | - [M5StickC本体例](https://www.switch-science.com/catalog/5517/) 301 | - [ピンヘッダ（オス）例](https://www.amazon.co.jp/dp/B012HY288S/) 302 | - [サーボ延長ケーブル例](https://www.amazon.co.jp/dp/B00W9ST610/) 303 | 304 | 305 | 以上 306 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | url: "https://hshin-git.github.io/GyroM5/" 3 | plugins: 4 | - jekyll-sitemap 5 | title: ラジドリ用ジャイロGyroM5 6 | description: M5StickCで自作するオープンソースのラジコン二駆ドリフト用ステアリングジャイロ 7 | locale: ja_JP 8 | languages: ['ja','en'] 9 | default_lang: 'ja' 10 | -------------------------------------------------------------------------------- /google6cdfcb07c2c314de.html: -------------------------------------------------------------------------------- 1 | google-site-verification: google6cdfcb07c2c314de.html 2 | --------------------------------------------------------------------------------