メカナムホイールのラジコン制作6

「メカナムホイールRCをアナログスティック1本で360度操作したい」と思ってから、はや5年(嘘)

ようやく実現できました!

PS2コントローラの左アナログスティックで360度全方位アナログ移動で、
右アナログスティックで左右旋回です。

ついでに、〇または□ボタンでブースト走行です!
モーターの定格電圧以上に電圧をかけるため、多用するとモーターが臭くなります。(笑)

以下プログラムのソースです

// ****************************************************************************
// 【Arduino + VS-C1 + メカナムホイール】
//   接続方法はfritzingの図を参照
// ****************************************************************************
// 2014.12.28 (Ver1.0) 新規作成
// 2015.01.xx (Ver2.0) 左アナログスティックで全方位移動
// ****************************************************************************
// 操作方法
//    左アナログスティック:全方位移動
//    右アナログスティック:左右旋回
//    ○ or □ ボタン:ブースト走行
// ****************************************************************************

#include <math.h>
#include <PS2X_lib.h>  //for v1.6

// PS2コントローラ関連 ------------------------------------------------------
PS2X ps2x; // create PS2 Controller Class
int error = 0; 
byte type = 0;
byte vibrate = 0;
int ANALOG_PLAY = 10;    // アナログスティクのニュートラル感度(0:あそび無し~10程度)
//
// DCモータードライバ関連 ---------------------------------------------------
//モーター名と番号の定数
const int M_FRONT_RIGHT = 1;  //前輪右
const int M_FRONT_LEFT = 2;   //前輪左
const int M_REAR_RIGHT = 3;   //後輪右
const int M_REAR_LEFT = 4;    //後輪左

//ピン番号定義
//モータドライバ1個目(前輪右)の4~6番ピンに繋がっているピン番号
const int PIN_MFR_4 = 3; //  ※PIN_MFR_4 は PWM であること
const int PIN_MFR_5 = 8; 
const int PIN_MFR_6 = 7;
//モータドライバ2個目(前輪左)の4~6番ピンに繋がっているピン番号
const int PIN_MFL_4 = 5; //  ※PIN_MFL_4 は PWM であること
const int PIN_MFL_5 = 4; 
const int PIN_MFL_6 = 2;
//モータドライバ3個目(後輪右)の4~6番ピンに繋がっているピン番号
const int PIN_MRR_4 = 6; //  ※PIN_MFR_4 は PWM であること
const int PIN_MRR_5 = 4; 
const int PIN_MRR_6 = 2;
//モータドライバ4個目(後輪左)の4~6番ピンに繋がっているピン番号
const int PIN_MRL_4 = 9; //  ※PIN_MFL_4 は PWM であること
const int PIN_MRL_5 = 8; 
const int PIN_MRL_6 = 7;

const boolean MFR_RFLG = true;  // モーター前輪右の正点・逆転フラグ
const boolean MFL_RFLG = true;  // モーター前輪左の正点・逆転フラグ
//const boolean MRR_RFLG = false;  // モーター後輪右の正点・逆転フラグ
//const boolean MRL_RFLG = false;  // モーター後輪左の正点・逆転フラグ

const float MOTOR_LIMIT = 0.5;  // モーターリミッター(valに掛ける係数。90%のとき0.9)
const float MOTOR_BOOST = 0.8;  // モーターブースト(valに掛ける係数。90%のとき0.9)
boolean flgBoost = false;       //ブースト走行フラグ:○ or □ボタンでtrue

// ****************************************************************************
// void setup 初期処理
// ****************************************************************************
void setup(){

  Serial.begin(9600);
  Serial.println("[Arduino + VS-C1 + MecanumWheel] Start");

  // PS2コントローラ関連 ------------------------------------------------------
  //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(13,11,10,12, true, true);   
  if(error == 1)
    Serial.println("No controller found.");
  else if(error == 2)
    Serial.println("Controller found but not accepting commands.");
  else if(error == 3)
    Serial.println("This controller isn't support.");

  // DCモータードライバ関連 ---------------------------------------------------
  pinMode(PIN_MFR_5, OUTPUT);
  pinMode(PIN_MFR_6, OUTPUT);
  pinMode(PIN_MFL_5, OUTPUT);
  pinMode(PIN_MFL_6, OUTPUT);
//  pinMode(PIN_MRR_5, OUTPUT);
//  pinMode(PIN_MRR_6, OUTPUT);
//  pinMode(PIN_MRL_5, OUTPUT);
//  pinMode(PIN_MRL_6, OUTPUT);
}

// ****************************************************************************
// void loop メイン処理
// ****************************************************************************
void loop(){

  if(error == 1){ //skip loop if no controller found
    return;
  }

  boolean MoveFlg = false;
  
  //アナログスティックの値
  int AnaLX=0;
  int AnaLY=0;
  int AnaRX=0;

  //モーター回転量
  int SpeedR=255;
  int SpeedL=255;

  //PSコントローラの値を取得
  ps2x.read_gamepad(false, vibrate);

  //アナログスティック左の値を取得する
  AnaLX = ps2x.Analog(PSS_LX)*2 -256; //-255~255の値にする
  //遊びの範囲内なら未入力扱いにする
  if( AnaLX>(-ANALOG_PLAY) && AnaLX<(+ANALOG_PLAY)){
    AnaLX=0;
  }
  AnaLY = ps2x.Analog(PSS_LY)*2 -256; //-255~255の値にする
  //遊びの範囲内なら未入力扱いにする
  if( AnaLY>(-ANALOG_PLAY) && AnaLY<(+ANALOG_PLAY)){
    AnaLY=0;
  }

  //アナログスティック右の値を取得する
  AnaRX = ps2x.Analog(PSS_RX)*2 -256; //-255~255の値にする
  //遊びの範囲内なら未入力扱いにする
  if( AnaRX>(-ANALOG_PLAY) && AnaRX<(+ANALOG_PLAY)){
    AnaRX=0;
  }

  //モーター回転量計算(車体旋回方向の計算)
  if(AnaRX>0){         //アナログスティック右に傾けている場合
    SpeedR=255-AnaRX;  //右モーターの回転を弱める
    SpeedL=255;
  }else if(AnaRX<0){   //アナログスティック左に傾けている場合
    SpeedR=255;
    SpeedL=255+AnaRX;  //左モーターの回転を弱める
  }
  //Serial.print("AnaRX=");
  //Serial.print(AnaRX, DEC);

  //○ or □ボタン:ブースト走行
  if(ps2x.Button(PSB_CIRCLE) || ps2x.Button(PSB_SQUARE)) {
    flgBoost=true;
  }else{
    flgBoost=false;
  }

  //左アナログスティック移動
  if( (AnaLX != 0 || AnaLY != 0) && MoveFlg==false) {
    MoveFlg=true;
    moveAnalog(AnaLX, AnaLY, SpeedL, SpeedR);
  }

  //何も操作されていないなら停止(自然減速)
  if(MoveFlg==false){
    //Serial.print("**MoveFlg=false**");
    motordrive(M_FRONT_RIGHT, 0);
    motordrive(M_FRONT_LEFT, 0);
    motordrive(M_REAR_RIGHT, 0);
    motordrive(M_REAR_LEFT, 0);
  }

//  Serial.println(";");
  delay(100);
} 

// ****************************************************************************
// 左アナログスティック移動
// AnaLX : 左アナログスティックX軸(-255~255)
// AnaLY : 左アナログスティックY軸(-255~255)
// SpeedL: 左モーター回転量
// SpeedR: 右モーター回転量
// ****************************************************************************
void moveAnalog(int AnaLX, int AnaLY, int SpeedL, int SpeedR){
  double SpeedFL = (double)SpeedL;  //前輪左
  double SpeedFR = (double)SpeedR;  //前輪右
  double SpeedRL = (double)SpeedL;  //後輪左
  double SpeedRR = (double)SpeedR;  //後輪右

  int Kakudo = 0;                 //左アナログスティックの角度(-180度~180度)
  double rad2do = 57.29577951472; //ラジアンから角度に変換する係数(=180/3.1415926535)
  double sa = 0;
  boolean ml=true;  //左モーター回転方向
  boolean mr=true;  //右モーター回転方向
  Serial.print("(");
  Serial.print(AnaLX, DEC);
  Serial.print(",");
  Serial.print(AnaLY, DEC);
  Serial.print(",");
  Serial.print(SpeedL, DEC);
  Serial.print(",");
  Serial.print(SpeedR, DEC);
  Serial.print("), ");

  //スティックの角度を計算する
  if(AnaLX!=0 || AnaLY!=0){
    Kakudo = (int)(atan2(AnaLX, AnaLY) * rad2do);
  }
  Serial.print(Kakudo, DEC);
  Serial.print(", ");

  //スティックの角度で分岐する
  if( Kakudo>=90 && Kakudo<=180 ){         //(1)90度~180度
    //90度~135度のとき、右モーター逆回転
    if( Kakudo<135 ){
      mr = false;
    }
    
    //135度との差を0~1にマップする
    sa = (double)abs(135-Kakudo) / 45;
    //モーター右の回転量に掛ける(減速する)
    SpeedFR *= sa;
    SpeedRL *= sa;
    
  }else if( Kakudo<-90 ){  //(2)-90度~-180度
    //-90度~-135度のとき、左モーター逆回転
    if( Kakudo>-135 ){
      ml = false;
    }
    
    //135度との差を0~1にマップする
    sa = (double)abs(-135-Kakudo) / 45;
    //モーター左の回転量に掛ける(減速する)
    SpeedFL *= sa;
    SpeedRR *= sa;
      
  }else if( Kakudo>=0 && Kakudo<=90 ){    //(3)0度~90度
    //右モーター逆回転
    mr = false;
    //0度~45度のとき、左モーター逆回転
    if( Kakudo<45 ){
      ml = false;
    }
    
    //45度との差を0~1にマップする
    sa = (double)abs(45-Kakudo) / 45;
    //モーター左の回転量に掛ける(減速する)
    SpeedFL *= sa;
    SpeedRR *= sa;

  }else{                                //(4)0度~-90度
    //0度~-45度のとき、右モーター逆回転
    if( Kakudo&gt-45 ){
      mr = false;
    }
    //左モーター逆回転
    ml = false;
    
    //-45度との差を0~1にマップする
    sa = (double)abs(-45-Kakudo) / 45;
    //モーター右の回転量に掛ける(減速する)
    SpeedFR *= sa;
    SpeedRL *= sa;

  }
  Serial.print("[");
  Serial.print(SpeedFL, DEC);
  Serial.print(", ");
  Serial.print(SpeedFR, DEC);
  Serial.print(", ");
  Serial.print(SpeedRL, DEC);
  Serial.print(", ");
  Serial.print(SpeedRR, DEC);
  Serial.print("], ");

  //※スティックの傾き量からモーター出力を減速
  //sa = (double)sqrt((long)AnaLX^2 + (long)AnaLY^2);   // 0~255
  sa = max(abs(AnaLX), abs(AnaLY));
  Serial.print(sa, DEC);
  sa = sa / 255;                  // 0~1
  SpeedFL *= sa;
  SpeedFR *= sa;
  SpeedRL *= sa;
  SpeedRR *= sa;

  //モーター回転
  motormode(M_FRONT_RIGHT, mr);
  motormode(M_FRONT_LEFT, ml);
  motordrive(M_FRONT_RIGHT, SpeedFR);
  motordrive(M_FRONT_LEFT, SpeedFL);
  motordrive(M_REAR_RIGHT, SpeedRR);
  motordrive(M_REAR_LEFT, SpeedRL);
  Serial.print("[");
  Serial.print(SpeedFL, DEC);
  Serial.print(", ");
  Serial.print(SpeedFR, DEC);
  Serial.print(", ");
  Serial.print(SpeedRL, DEC);
  Serial.print(", ");
  Serial.print(SpeedRR, DEC);
  Serial.print("], ");
  Serial.println(";");

}

// ****************************************************************************
// モーター制御
// motor : 動かすモーター番号(1 to 4)
// val   : 回転量(0~255)
// ****************************************************************************
void motordrive(int motor, int val){
  int pin4;

 //Serial.print("void motordrive: ");
  //Ardino pin 設定
  switch(motor){  
  case M_FRONT_RIGHT:
    pin4 = PIN_MFR_4;
    break;
  case M_FRONT_LEFT:
    pin4 = PIN_MFL_4;
    break;
  case M_REAR_RIGHT:
    pin4 = PIN_MRR_4;
    break;
  case M_REAR_LEFT:
    pin4 = PIN_MRL_4;
    break;
  }  

  if(flgBoost==true){
    val = val*MOTOR_BOOST;    // ブースト係数を掛ける
  }else{
    val = val*MOTOR_LIMIT;    // リミッター係数を掛ける
  }
  analogWrite(pin4, val);    //出力値:0~255

  //Serial.print("val2=");
  //Serial.print(val, DEC);
  //Serial.println(";");
}

// ****************************************************************************
// モーター回転方向制御
// motor : 動かすモーター番号(1 to 2)
// val   : 正回転 true/false
// ****************************************************************************
void motormode(int motor, boolean val){
  int pin5;
  int pin6;

 //Serial.print("void motormode: ");
  //Ardino pin 設定
  switch(motor){  
  case M_FRONT_RIGHT:
    if( MFR_RFLG==false ){
      pin5 = PIN_MFR_5;
      pin6 = PIN_MFR_6;
     //Serial.print("mode1a: ");
    }
    else{
      pin5 = PIN_MFR_6;
      pin6 = PIN_MFR_5;
     //Serial.print("mode1b: ");
    }
    break;
  case M_FRONT_LEFT:
    if( MFL_RFLG==false ){
      pin5 = PIN_MFL_5;
      pin6 = PIN_MFL_6;
     //Serial.print("mode2a: ");
    }
    else{
      pin5 = PIN_MFL_6;
      pin6 = PIN_MFL_5;
     //Serial.print("mode2b: ");
    }
    break;
  }  

  if( val == true){  //正転
   //Serial.print("move1: ");
    digitalWrite(pin5,HIGH);
    digitalWrite(pin6,LOW);
  }
  else {    //逆転
   //Serial.print("move2: ");
    digitalWrite(pin5,LOW);
    digitalWrite(pin6,HIGH);
  }
}



コメントを残す

メールアドレスが公開されることはありません。


*