メカナムホィール 車 に M5StickC を搭載し PS3コントローラ で操作し遊んでみた。
◆ブログ内参考
・I2C Wemos D1 mini Motor Shield を使かってみた
・ESP32 (M5StickC) で PS3 の コントローラ を使う
◆全体写真
◆メカナムホィールの動きとPS3コントローラの釦割り当て
◆配線略図
◆アマゾンで購入
Homyl DIY メカナムホイール 4輪 ロボットカーシャーシ キット STEM知育玩具 Arduino用
◆メカナムホィールを取り付けた所
◆モーターシールドのI2Cアドレスを表示させた所
◆PS3コントローラのバッテりー残量表示させた所
◆スケッチ例(あくまでも参考です。自己責任で)
◆ブログ内参考
・I2C Wemos D1 mini Motor Shield を使かってみた
・ESP32 (M5StickC) で PS3 の コントローラ を使う
◆全体写真
◆メカナムホィールの動きとPS3コントローラの釦割り当て
◆配線略図
◆アマゾンで購入
Homyl DIY メカナムホイール 4輪 ロボットカーシャーシ キット STEM知育玩具 Arduino用
◆メカナムホィールを取り付けた所
◆モーターシールドのI2Cアドレスを表示させた所
◆PS3コントローラのバッテりー残量表示させた所
◆スケッチ例(あくまでも参考です。自己責任で)
//PS3コントローラ対応 //2020/8/22 メカナムホイール車用に改修 //http://gijin77.blog.jp/archives/26057910.html #include <Wire.h> #include <M5StickC.h> #include <Ps3Controller.h> #include "WEMOS_Motor.h" /* #define _CCW 1 #define _BK 2 #define _STOP 3 #define _STANDBY 4 */ #define _FD 1 #define _BK 2 #define BTN_A 37 #define BTN_B 39 #define LED 10 // Global Variables float vBatt, voltAve=3.7; int bx=10; int by=80; int bw=20; int bh=64; int stop_flg=0; int mrx=0; int mry=0; int mlx=0; int mly=0; int demoflg=0; int pwr=50; int xyoff=50; int ps3bat; //PS3バッテリー残量 //Motor shield default I2C Address: 0x2F,0x30 //PWM frequency: 1000Hz(1kHz) Motor MFR(0x2F, _MOTOR_A, 1000); //Motor A FR Motor MFL(0x2F, _MOTOR_B, 1000); //Motor B FL Motor MBR(0x30, _MOTOR_A, 1000); //Motor A BR Motor MBL(0x30, _MOTOR_B, 1000); //Motor B BL void setup() { pinMode(BTN_A, INPUT); pinMode(BTN_B, INPUT); pinMode(LED, OUTPUT); digitalWrite(LED,HIGH); Serial.begin(115200); M5.begin(); Wire.begin(0, 26); //SDA,SCL Serial.println("プログラム 開始"); Ps3.begin("D8:A0:1D:58:4E:C6"); for (int i=0;i<3;i++){ M5.Lcd.fillScreen(GREEN); digitalWrite(LED,LOW);delay(250); M5.Lcd.fillScreen(BLACK); digitalWrite(LED,HIGH);delay(250); } M5.Lcd.setRotation( 0 ); //Mycar = 0 M5.Lcd.setTextFont(1); M5.Lcd.setTextSize(2); M5.Lcd.setTextColor(GREEN); M5.Lcd.setCursor(5,0);M5.Lcd.print("Manual"); dispvbatt(); face(); } void loop() { int rx,ry; int lx,ly; M5.update(); if ( M5.Axp.GetBtnPress() ) { // 電源ボタンを押すとパワーオフ // M5.Axp.pwrerOff(); } if ( M5.BtnB.wasReleased() ) { // Bボタンを押すとリスタート esp_restart(); } if ( M5.BtnA.wasReleased() ) { // Aボタンを押すとデモスタート demo_a(); } if (Ps3.isConnected()){ dispvbatt(); if (Ps3.data.button.l1){ // Serial.println("l1 ボタン圧下"); ps3bat=Ps3.data.status.battery; disp3batt(ps3bat); lcdout(GREEN,"Manual"); delay(200);demoflg=0; } else if (Ps3.data.button.l2){ //バッテリ表示デモ // Serial.println("l2 ボタン圧下"); for (int i=1;i<6;i++){ disp3batt(i); } disp3batt(0xee); lcdout(GREEN,"Manual"); delay(200);demoflg=0; } else if (Ps3.data.button.select ){ // Serial.println("select ボタン圧下"); i2cck(); } else if (Ps3.data.button.triangle){ // Serial.println("triangle ボタン圧下"); lcdout(WHITE,"DEMO A");delay(200);demoflg=1; } else if (Ps3.data.button.circle){ // Serial.println("circle ボタン圧下"); lcdout(WHITE,"DEMO B");delay(200);demoflg=2; } else if (Ps3.data.button.cross){ // Serial.println("cross ボタン圧下"); lcdout(WHITE,"DEMO C");delay(200);demoflg=3; } else if (Ps3.data.button.square){ lcdout(WHITE,"DEMO D");delay(200);demoflg=4; // Serial.println("square ボタン圧下"); } else if (Ps3.data.button.r3){ // Serial.println("r3 ボタン圧下"); //デモリセット lcdout(GREEN,"Manual"); delay(200);demoflg=0; } else if (Ps3.data.button.start){ // Serial.println("start ボタン圧下"); //デモスタート if (demoflg==1) demo_a(); else if (demoflg==2) demo_b(); else if (demoflg==3) demo_c(); else if (demoflg==4) demo_d(); } else if (Ps3.data.button.r1){ // Serial.println("r1 ボタン圧下"); lcdout(YELLOW,"pwr 50"); pwr=50;delay(200); } else if (Ps3.data.button.r2){ // Serial.println("r2 ボタン圧下"); lcdout(YELLOW,"pwr100"); pwr=100;delay(200); } rx=Ps3.data.analog.stick.rx; ry=Ps3.data.analog.stick.ry; lx=Ps3.data.analog.stick.lx; ly=Ps3.data.analog.stick.ly; if (abs(rx)<xyoff) rx=0; if (abs(ry)<xyoff) ry=0; if (abs(lx)<xyoff) lx=0; if (abs(ly)<xyoff) ly=0; rx = map(rx,-128,128,100, -100); // R-JOYSTICK X ry = map(ry,-128,128,100, -100); // R-JOYSTICK Y lx = map(lx,-128,128,100, -100); // L-JOYSTICK X ly = map(ly,-128,128,100, -100); // L-JOYSTICK Y if ((rx==0)&&(ry==0)&&(lx==0)&&(ly==0)&&(stop_flg==0)) { stop_flg=1; drvFR(_STOP,0); drvBR(_STOP,0); drvFL(_STOP,0); drvBL(_STOP,0); face(); } else if ((mrx != rx)||(mry != ry)||(mlx != lx)||(mly != ly)){ Serial.printf("1 RX=%4d RY=%4d LX=%4d LY=%4d pwrer=%d",rx,ry,lx,ly,pwr); Serial.println(); stop_flg=0; BLE_4car(rx,ry,lx,ly); } mrx=rx;mry=ry;mlx=lx;mly=ly; } delay(100); } void i2cck(){ int address; int error; M5.Lcd.fillScreen(BLACK); M5.Lcd.setTextSize(1); M5.Lcd.setCursor(0, 0); M5.Lcd.println("I2C scanning"); Serial.println("I2C address scanning"); for(address = 1; address < 127; address++ ) { Wire.beginTransmission(address); error = Wire.endTransmission(); if(error==0) { M5.Lcd.print(address,HEX);M5.Lcd.print(" "); Serial.println(address,HEX); } else { M5.Lcd.print("."); // Serial.println("."); } delay(10); } Serial.println("End"); delay(3000); M5.Lcd.setTextSize(2); M5.Lcd.fillScreen(BLACK); lcdout(GREEN,"Manual"); } void BLE_4car(int rx,int ry,int lx,int ly) { //int pwr=50; /* +Y +X -X -Y */ if ((ry> xyoff)&&(rx==0)) { ff(pwr);Serial.print("ff");}//前進 if ((ry<-xyoff)&&(rx==0)) { bb(pwr);Serial.print("bb");}//後進 if ((rx<-xyoff)&&(ry==0)) { ll(pwr);Serial.print("ll");}//左横移動 if ((rx> xyoff)&&(ry==0)) { rr(pwr);Serial.print("rr");}//右横移動 if ((ry> xyoff)&&(rx> xyoff)) { fr(pwr);Serial.print("fr");}//右前斜め if ((ry> xyoff)&&(rx<-xyoff)) { fl(pwr);Serial.print("fl");}//左前斜め if ((ry<-xyoff)&&(rx> xyoff)) { br(pwr);Serial.print("br");}//右後斜め if ((ry<-xyoff)&&(rx<-xyoff)) { bl(pwr);Serial.print("bl");}//左後斜め if ((ly> xyoff)&&(lx==0)) { ff(pwr);Serial.print("ff");}//前進 if ((ly<-xyoff)&&(lx==0)) { bb(pwr);Serial.print("bb");}//後進 if ((lx<-xyoff)&&(ly==0)) { rcir(pwr);Serial.print("rcir"); }//右旋回 if ((lx> xyoff)&&(ly==0)) { lcir(pwr);Serial.print("lcir"); }//左旋回 if ((lx<-xyoff)&&(ly> xyoff)) { ftr(pwr);Serial.print("ftr");} //前右折 Turn right if ((lx> xyoff)&&(ly> xyoff)) { ftl(pwr);Serial.print("ftl"); }//前左折 Turn left if ((lx<-xyoff)&&(ly<-xyoff)) { btr(pwr);Serial.print("btr"); }//後右折 if ((lx> xyoff)&&(ly<-xyoff)) { btl(pwr);Serial.print("btl"); }//後左折 Serial.println(" --> end "); } void drvFR(int fb,int po){ int h; if (po>bh) h=bh; else h=po; MFR.setmotor( fb,po); M5.Lcd.fillRect(bx+30, by-bh-2, 40, bh, BLACK); if (fb== _BK) { M5.Lcd.fillRect(bx+50, by-h-2, bw, h, RED); } else { M5.Lcd.fillRect(bx+50, by-h-2, bw, h, GREEN); } } void drvFL(int fb,int po){ int h; if (po>bh) h=bh; else h=po; MFL.setmotor( fb, po); M5.Lcd.fillRect(bx-10, by-bh-2, 40, bh, BLACK); if (fb==_BK) { M5.Lcd.fillRect(bx, by-h-2, bw, h, RED); } else { M5.Lcd.fillRect(bx, by-h-2, bw, h, GREEN); } } void drvBR(int fb,int po){ int h; if (po>bh) h=bh; else h=po; MBR.setmotor( fb,po); M5.Lcd.fillRect(bx+30, by+2, 40, bh, BLACK); if (fb== _BK) { M5.Lcd.fillRect(bx+50, by+2, bw, h, RED); } else { M5.Lcd.fillRect(bx+50, by+2, bw, h, GREEN); } } void drvBL(int fb,int po){ int h; if (po>bh) h=bh; else h=po; MBL.setmotor( fb, po); M5.Lcd.fillRect(bx-10, by+2, 40, bh, BLACK); if (fb==_BK) { M5.Lcd.fillRect(bx, by+2, bw, h, RED); } else { M5.Lcd.fillRect(bx, by+2, bw, h, GREEN); } } void ff(int pwr){//前進 drvFR( _FD, pwr); drvFL( _FD, pwr); drvBR( _FD, pwr); drvBL( _FD, pwr); } void bb(int pwr){//後進 drvFR( _BK, pwr); drvFL( _BK, pwr); drvBR( _BK, pwr); drvBL( _BK, pwr); } void rr(int pwr){//右横移動 drvFR( _BK, pwr); drvFL( _FD, pwr); drvBR( _FD, pwr); drvBL( _BK, pwr); } void ll(int pwr){//左横移動 drvFR( _FD, pwr); drvFL( _BK, pwr); drvBR( _BK, pwr); drvBL( _FD, pwr); } void fr(int pwr){//右前斜め drvFR(_STOP,0); drvFL( _FD, pwr); drvBR( _FD, pwr); drvBL(_STOP,0); } void fl(int pwr){//左前斜め drvFR( _FD, pwr); drvFL(_STOP,0); drvBR(_STOP,0); drvBL( _FD, pwr); } void br(int pwr){//右後斜め drvFR( _BK, pwr); drvFL(_STOP,0); drvBR(_STOP,0); drvBL( _BK, pwr); } void bl(int pwr){//左後斜め drvFR(_STOP,0); drvFL( _BK, pwr); drvBR( _BK, pwr); drvBL(_STOP,0); } void rcir(int pwr){ drvFR( _BK, pwr);//右旋回 drvFL( _FD, pwr); drvBR( _BK, pwr); drvBL( _FD, pwr); } void lcir(int pwr){//左旋回 drvFR( _FD, pwr); drvFL( _BK, pwr); drvBR( _FD, pwr); drvBL( _BK, pwr); } void ftr(int pwr){//前右折 drvFR(_STOP,0); drvFL( _FD, pwr); drvBR(_STOP,0); drvBL( _FD, pwr); } void ftl(int pwr){//前左折 drvFR( _FD, pwr); drvFL(_STOP,0); drvBR( _FD, pwr); drvBL(_STOP,0); } void btr(int pwr){//後右折 drvFR(_STOP,0); drvFL( _BK, pwr); drvBR(_STOP,0); drvBL( _BK, pwr); } void btl(int pwr){//後左折 drvFR( _BK, pwr); drvFL(_STOP,0); drvBR( _BK, pwr); drvBL(_STOP,0); } void mstop(){ drvFR(_STOP,0); drvFL(_STOP,0); drvBR(_STOP,0); drvBL(_STOP,0); face(); delay(1000); } void demo_a(){ int pwr=50; int wt=1000; lcdout(GREEN,"DEMO A"); //前進 ff(pwr);delay(wt/2);mstop(); //後進 bb(pwr);delay(wt);mstop(); //前進 ff(pwr);delay(wt/2);mstop(); //右横移動 rr(pwr);delay(wt/2);mstop(); //左横移動 ll(pwr);delay(wt);mstop(); //右横移動 rr(pwr);delay(wt/2);mstop(); //右旋回 rcir(pwr);delay(wt*3);mstop(); //左旋回 lcir(pwr);delay(wt*3);mstop(); //右前斜め fr(pwr);delay(wt/2);mstop(); //左後斜め bl(pwr);delay(wt/2);mstop(); //左前斜め fl(pwr);delay(wt/2);mstop(); //右後斜め br(pwr);delay(wt/2);mstop(); demoend(); } void demo_b() { int pwr=50; int wt=1000; lcdout(GREEN,"DEMO B"); //前進 ff(pwr);delay(wt/2);mstop(); //右横移動 rr(pwr);delay(wt/2);mstop(); //後進 bb(pwr);delay(wt);mstop(); //左横移動 ll(pwr);delay(wt);mstop(); //前進 ff(pwr);delay(wt);mstop(); //右横移動 rr(pwr);delay(wt/2);mstop(); //後進 bb(pwr);delay(wt/2);mstop(); demoend(); } void demo_c(){ int pwr=75; int wt=700; lcdout(GREEN,"DEMO C"); //前進 ff(pwr);delay(wt); //右横移動 rr(pwr);delay(wt); //後進 bb(pwr);delay(wt); //左横移動 ll(pwr);delay(wt); //右横移動 rr(pwr);delay(wt); //前進 ff(pwr);delay(wt); //左横移動 ll(pwr);delay(wt); //後進 bb(pwr);delay(wt); //右前斜め fr(pwr);delay(wt/2); //右旋回 rcir(pwr);delay(wt*3); //左旋回 lcir(pwr);delay(wt*3); //左後斜め bl(pwr);delay(wt/2); demoend(); } void demo_d(){ int pwr=75; int wt=500; lcdout(GREEN,"DEMO D"); //前進 ff(pwr);delay(wt); //右横移動 rr(pwr);delay(wt); //後進 bb(pwr);delay(wt); //左横移動 ll(pwr);delay(wt); //右横移動 rr(pwr);delay(wt); //前進 ff(pwr);delay(wt); //左横移動 ll(pwr);delay(wt); //後進 bb(pwr);delay(wt); //右前斜め fr(pwr);delay(wt/2); //右旋回 rcir(pwr);delay(wt*3); //左旋回 lcir(pwr);delay(wt*3); //左後斜め bl(pwr);delay(wt/2); demoend(); } void demoend(){ mstop(); lcdout(GREEN,"Manual"); demoflg=0; } void lcdout(int col,String msg){ M5.Lcd.fillRect(0,0,79,32, BLACK); M5.Lcd.setCursor(5, 0); M5.Lcd.setTextColor(col); M5.Lcd.print(msg); } void face(){ int x0=10; int y0=40; int w0=25; int h0=10; int col=TFT_ORANGE; M5.Lcd.fillRect(x0,y0,w0,h0,col); M5.Lcd.fillRect(x0+35,y0,w0,h0,col); M5.Lcd.fillCircle(x0+12,y0+25, 7,col); M5.Lcd.fillCircle(x0+12,y0+25, 5,BLACK); M5.Lcd.fillCircle(x0+48,y0+25, 7,col); M5.Lcd.fillCircle(x0+48,y0+25, 5,BLACK); M5.Lcd.fillTriangle(x0+30,y0+25, 30,90,50,90,col); M5.Lcd.fillRect(20,y0+60,40,15,col); } void dispvbatt(){ vBatt= M5.Axp.GetVbatData() * 1.1 / 1000; M5.Lcd.fillRect(0,145,79,32, BLACK); M5.Lcd.setCursor(10,145); if (vBatt > voltAve) { M5.Lcd.setTextColor(GREEN); } else{ M5.Lcd.setTextColor(RED); } M5.Lcd.printf("%4.2fv ", vBatt); } void disp3batt(int p3bt){ int x0=20; int y0=30; int w0=40; int h0=120; int y1=22; int x1,w1; int i; // M5.Lcd.fillRect(0,y0,79,h0,BLACK); M5.Lcd.fillScreen(BLACK); dispvbatt(); lcdout(GREEN,"PS3Bat"); x1=x0+1;w1=w0-2; if (p3bt==0xEE) { for(int j=0;j<6;j++){ M5.Lcd.fillRect(x1,y0,w1,y1*5,GREEN); for(i=0;i<5;i++){ M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE); } delay(500); M5.Lcd.fillRect(x1,y0,w1,y1*5,BLACK); for(i=0;i<5;i++){ M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE); } delay(500); } M5.Lcd.fillScreen(BLACK); return; } i=5-p3bt; switch(p3bt){ case 1: M5.Lcd.fillRect(x1,y0+y1*i,w1,20,RED); break; case 2: M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,YELLOW); break; case 3: M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,YELLOW); break; case 4: M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,GREEN); break; case 5: M5.Lcd.fillRect(x1,y0+y1*i,w1,y1*p3bt,GREEN); break; } for(i=0;i<5;i++){ M5.Lcd.drawRect(x0,y0+y1*i,w0,20,WHITE); } delay(3000); // M5.Lcd.fillRect(0,y0,79,h0,BLACK); M5.Lcd.fillScreen(BLACK); }
コメント