読者です 読者をやめる 読者になる 読者になる

Cafe'Tec Yagiyu

のんびり気ままに技術進捗

中ロボ終了!&メール通知機の開発状況(1)

さて、つい先日の3/8に、うちの部内でのロボット大会である、中ロボが終了しました!

ところで、中ロボとはどのような大会なのかというと...

 ・部内の1,2年生が2チームに分かれてロボットを制作し、決められた競技をする大会で、技術向上を目的として行われるものである

 ・NHK主催の高専ロボコンより少し簡易的な競技を行う

 

というもので、今年の内容としては...

 ・テーマは物資運送で、輪っかゾーン、平均台ゾーン、S字カーブゾーンを越えてどれだけ物資を運べるかを競うものだった

 ・ロボットのサイズは600x600xh(高さ無制限)

 ・コントローラは無線縛り

という感じでした。

 

1月末から制作を開始して、2月のテスト期間を経て3月8日に競技大会、というスケジュールの中、それぞれロボットを制作しました。

僕は回路班の主体となって制作を行い、メイン基板、エアーシリンダの制御のための電磁弁制御基板、モータードライバ制御の基板、およびそれぞれのプログラムをしました。

後輩二人は無線モジュールとコントローラのプログラミング、回路板、基板制作をしてくれました。

そして、実際に制作したロボットがこちらです

f:id:yagiyu-tec:20160310205322j:plain

f:id:yagiyu-tec:20160310205423j:plain

 

ここで、それぞれ使用したモジュールを紹介します。

 ・制御ボード...ArduinoUNO×2

Arduino - ArduinoBoardUno

 

 ・無線通信...USBホストシールド2.0 for Arduino

www.amazon.co.jp

 

 ・モータードライバ...VNH3SP30 MD01B(Pololu)

www.pololu.com

 

 ・コントローラ...PS3コントローラ DualShock3

www.amazon.co.jp

 

 ・電源...Li-Po二次電池 3セル 11.1V 3500mAh 25C ×2

www.enroute.co.jp

 

 電磁弁駆動にはC1815(http://www.op316.com/tubes/tips/image/2sc1815.pdf)を使いました。

 

書いたプログラムも載せておきます。

○メインプログラム(モータードライバ制御、電磁弁制御プログラムも含む)

//////////////////////////////////////////////////////////////////
//	Pro Mini	B_Main		3/7 18:33		//
//////////////////////////////////////////////////////////////////

int air1 = 2; //電磁弁1
int air2 = 3;//電磁弁2
int air3 = 4; //電磁弁3
int m1i1 = 6;		// m1in1はデジタル4番ピン
int m1i2 = 7;          // m1in2はデジタル2番ピン
int m1p = 5;		// m1pwmはデジタル3番ピン

int m2i1 = 8;		// m2in1はデジタル5番ピン
int m2i2 = 10;		// m2in2はデジタル7番ピン
int m2p = 9;		// m2pwmはデジタル5番ピン

int m3i1 = A0;		// m3in1はアナログA0番ピン
int m3i2 = A1;		// m3in2はアナログA1番ピン
int m3p = 11;		// m3pwmはデジタル11番ピン

#include wire.h

//////////////////////////////////////////////////////////////////
//	定数							//
//////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////
//	グローバル変数						//
//////////////////////////////////////////////////////////////////
byte ConData[4];
int g_m1 = 0;
int g_m2 = 0;
byte movR=0;
byte movL=0;
byte mSig[2];

boolean mFlag = 0;

void setup()		// 初期設定
{
  //Wire.begin();                // join i2c bus for Mastor
  //ControlGet();
//  Wire.onRequest(requestEvent); // register event
  pinMode(air1, OUTPUT);
  pinMode(air2, OUTPUT);
  pinMode(air3, OUTPUT);
  
  pinMode(m1i1, OUTPUT);		// 出力に設定
  pinMode(m1i2, OUTPUT);		// 出力に設定
  pinMode(m1p, OUTPUT);		// 出力に設定(PWM)

  pinMode(m2i1, OUTPUT);		// 出力に設定
  pinMode(m2i2, OUTPUT);		// 出力に設定
  pinMode(m2p, OUTPUT);		// 出力に設定(PWM)

  pinMode(m3i1, OUTPUT);		// 出力に設定
  pinMode(m3i2, OUTPUT);		// 出力に設定
  pinMode(m3p, OUTPUT);
  
  Wire.begin();                // join i2c bus for Mastor
  //ControlGet();
  
  Serial.begin(9600);
}

void loop()
{
  ControlGet();
  MotorDrive();
  AirDrive();
}


void ControlGet()	// コントローラの値を取得
{
  //device #2 is WirelesReader Device
  //device #3 is MotorDrive Device
  //device #4 is AirDrive Device
Wire.requestFrom(2, 4);  // request 4 bytes from slave device #2
  
  //digitalWrite(7, HIGH);
  
 while(Wire.available()>0)
{
  ConData[0] = Wire.read();  //Air Signal 3bit
  ConData[1] = Wire.read();  //4bit RLAH 4bit Value 1/O 0/F
  ConData[2] = Wire.read(); //Right PWM
  ConData[3] = Wire.read();  //Left PWM
  
  Serial.println(ConData[0],BIN);
  Serial.println(ConData[1],BIN);
  Serial.println(ConData[2]);
  Serial.println(ConData[3]);
  Serial.println("");
}
}

void MotorDrive()
{
if(bitRead(ConData[0], 5) == 1) //bitRead:右端からn(0-7)番目のデータを読む
{
  byte x;
  
  x = ConData[1];
  movR = ConData[2]-80;
  movL = ConData[3]-60;
  
  
  for(byte i=0;i<2;i++){
    mSig[i] = x>>4*i;
  }
  mSig[0] &= B00001111;
  
  mFlag = 1;
  
  if(mFlag == 1){
    motorDrive(movR, movL);
    
    mFlag = 0;
  }
}else{
  m1_FR();
  m2_FR();
  m3_FR();
}
}

void motorDrive(int mp_1, int mp_2){
  switch(mSig[1]){
    case 0:
     m1_FR();
     m2_FR();
     m3_FR();
  case 8:  //Right leg move
    if(mSig[0] == 0){
      m1_F(mp_1);          //m1_IN
      m2_FR();
      m3_FR();
      
     // Serial.println("M1 move");
    }else if(mSig[0] == 8){
      m1_O(mp_1);
      m2_FR();
      m3_FR();
      
      //Serial.println("M1 move");
    }
    break;
  case 4:  //Left leg move
    if(mSig[0] == 0){
      m1_FR();
      m2_F(mp_2);      //m2 IN
      m3_FR();
      
      //Serial.println("M2 move");
    }else if(mSig[0] == 4){
      m1_FR();
      m2_O(mp_2);
      m3_FR();
      
     // Serial.println("M2 move");
    }
    break;
  case 12:  //both leg move
    if(mSig[0] == 0){
      m1_F(mp_1);
      m2_F(mp_2);
      m3_FR();
    }else if(mSig[0] == 8){
      m1_O(mp_1);
      m2_F(mp_2);
      m3_FR();
    }else if(mSig[0] == 4){
      m1_F(mp_1);
      m2_O(mp_2);
      m3_FR();
    }else if(mSig[0] == 12){
      m1_O(mp_1);
      m2_O(mp_2);
      m3_FR();
    }
    break;
  case 2:  //Arm move
    if(mSig[0] == 0){
      m1_FR();
      m2_FR();
      m3_F();
      
     // Serial.println("M3 move");
    }else if(mSig[0] == 2){
      m1_FR();
      m2_FR();
      m3_O();
      
     // Serial.println("M3 move");
    }
    break;
  case 10:  //Right leg and Arm move
    if(mSig[0] == 0){
      m1_F(mp_1);
      m2_FR();
      m3_F();
    }else if(mSig[0] == 8){
      m1_O(mp_1);
      m2_FR();
      m3_F();
    }else if(mSig[0] == 2){
      m1_F(mp_1);
      m2_FR();
      m3_O();
    }else if(mSig[0] == 10){
      m1_O(mp_1);
      m2_FR();
      m3_O();
    }
    break;
  case 6:  //Left leg and Arm move
    if(mSig[0] == 0){
      m1_FR();
      m2_F(mp_2);
      m3_F();
    }else if(mSig[0] == 4){
      m1_FR();
      m2_O(mp_2);
      m3_F();
    }else if(mSig[0] == 2){
      m1_FR();
      m2_F(mp_2);
      m3_O();
    }else if(mSig[0] == 6){
      m1_FR();
      m2_O(mp_2);
      m3_O();
    }
    break;
  case 14:  //both leg and Arm move
    if(mSig[0] == 0){
      m1_F(mp_1);
      m2_F(mp_2);
      m3_F();
    }else if(mSig[0] == 8){
      m1_O(mp_1);
      m2_F(mp_2);
      m3_F();
    }else if(mSig[0] == 4){
      m1_F(mp_1);
      m2_O(mp_2);
      m3_F();
    }else if(mSig[0] == 2){
      m1_F(mp_1);
      m2_F(mp_2);
      m3_O();
    }else if(mSig[0] == 12){
      m1_O(mp_1);
      m2_O(mp_2);
      m3_F();
    }else if(mSig[0] == 10){
      m1_O(mp_1);
      m2_F(mp_2);
      m3_O();
    }else if(mSig[0] == 6){
      m1_F(mp_1);
      m2_O(mp_2);
      m3_O();
    }else if(mSig[0] == 14){
      m1_O(mp_1);
      m2_O(mp_2);
      m3_O();
    }
    break;
  }
}

void m1_F(int m1){  //Forward Rotate
  digitalWrite(m1i1, LOW);	//m1定義(正転)
  digitalWrite(m1i2, HIGH);
  analogWrite(m1p,m1);
}

void m1_O(int m1){  //Opposite Rotate
  digitalWrite(m1i1, HIGH);	//m1定義(逆転)
  digitalWrite(m1i2, LOW);
  analogWrite(m1p,m1);
}

void m1_FR(){
  digitalWrite(m1i1, LOW);
  digitalWrite(m1i2, LOW);
}

void m2_F(int m2){
  digitalWrite(m2i1, LOW);	//m2(逆転)
  digitalWrite(m2i2, HIGH);
  analogWrite(m2p,m2);
}

void m2_O(int m2){
  digitalWrite(m2i1, HIGH);	//m2(正転)
  digitalWrite(m2i2, LOW);
  analogWrite(m2p,m2);
}

void m2_FR(){
  digitalWrite(m2i1, LOW);
  digitalWrite(m2i2, LOW);
}

void m3_F(){
  digitalWrite(m3i1, LOW);	//m3(正転)
  digitalWrite(m3i2, HIGH);
  analogWrite(m3p,55);
}

void m3_O(){
  digitalWrite(m3i1, HIGH);	//m3(逆転)
  digitalWrite(m3i2, LOW);
  analogWrite(m3p,55);
  
}

void m3_FR(){
  digitalWrite(m3i1, LOW);
  digitalWrite(m3i2, LOW);
}

void AirDrive()
{
  if(bitRead(ConData[0], 4) == 1){
    digitalWrite(air1, bitRead(ConData[0], 3));
    digitalWrite(air2, bitRead(ConData[0], 2));
    digitalWrite(air3, bitRead(ConData[0], 1));
  }else{
    digitalWrite(air1, LOW);
    digitalWrite(air2, LOW);
    digitalWrite(air3, LOW);
}
}

 

○コントローラーの受信プログラム(後輩の書いたものなんで、一応許可は取りました)

/*
 Example sketch for the PS3 Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */

#include ps3bt.h
#include usbhub.h
#include wire.h

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include spi4teensy3.h
#include spi.h
#endif

USB Usb;
//USBHub Hub1(&Usb); // Some dongles have a hub inside

BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the class in two ways */
PS3BT PS3(&Btd); // This will just create the instance
//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch



byte Data=0;
byte Data2=0;


boolean moter = false;        //モーターが一つでも動くかどうか(1byte目)
boolean moterR=false;         //右モーターが動くかどうか(2byte目)
boolean moterR_D=false;       //右モーターの回転方向(2byte目)
boolean moterL=false;         //左モーターが動くかどうか(2byte目)
boolean moterL_D=false;       //左モーターの回転方向(2byte目)
boolean Arm_Data=false;       //腕が動くかどうか(2byte目)
boolean Arm_Data_D=false;     //腕の上下(2byte目)
boolean Hand_Data=false;      //手が動くかどうか(2byte目)
boolean Hand_Data_D=false;    //手、つかむ or 放す(2byte目)

boolean valve = false;
boolean valve1 = false;
boolean valve2 = false;
boolean valve3 = false;

boolean valve_If1 = false;
boolean valve_If2 = false;
boolean valve_If3 = false;
boolean valveT = false;


boolean Lair = false;
boolean Lair1 = false;
boolean Lair2 = false;
boolean Lair3 = false;
boolean Lair_T = false;
boolean Lair_If1 = false;
boolean Lair_If2 = false;
boolean Lair_count = false;

boolean valve_count = false;

boolean Lock = false;
boolean Lock_count = false;

byte pwmR=0;
byte pwmL=0;

bool printTemperature;
bool printAngle;

void setup() {
  Wire.onRequest(requestEvent);
  Wire.begin(2);
  Serial.begin(115200);
#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));
}



 void requestEvent(){
  byte x[4];
  x[0]=Data;
  x[1]=Data2;
  x[2]=pwmR;
  x[3]=pwmL;
  //digitalWrite(7,HIGH);
  
  Wire.write(x,4);
 }
 



void loop() {
  Usb.Task();


        
      if (PS3.getButtonPress(TRIANGLE)){
         moter=1;
         moterL=1;
         moterL_D=0;
      }
      
      if (PS3.getButtonPress(CIRCLE)){
        moter=1;
        moterR=1;
        moterR_D=0;
      }
      
      if (PS3.getButtonPress(CROSS)){
        moter=1;
        moterR=1;
        moterR_D=1;
      }
      if (PS3.getButtonPress(SQUARE)){
        moter=1;
        moterL=1;
        moterL_D=1;
      }

      if (PS3.getButtonPress(UP)) {
        moter=1;
        Arm_Data=1;
        Arm_Data_D=1;
      }
        
      
      if (PS3.getButtonClick(RIGHT)) {
        
       if(valve_count==0){
        valve=1;
        valve1=1;
        valve2=1;
        valveT=1;
        valve_If1=1;
        valve_If2=1;
        valve_count = 1;
       }else{
        valve1=0;
        valve2=0;
        valveT=0;
        valve_If1=0;
        valve_If2=0;
        valve_count=0;
       }
         //Hand_Data=1;
         //Hand_Data_D=1;
      }
     
      if (PS3.getButtonPress(DOWN)) {
       moter=1;
       Arm_Data=1;
       Arm_Data_D=0;
        }
      
      if (PS3.getButtonClick(LEFT)) {

        if(Lock_count==0){
        Lock=1;
        Lock_count=1;
        }else{
          Lock=0;
          Lock_count=0;
        }
        
//        valve1=0;
//        valve2=0;
//        valveT=0;
//        valve_If1=0;
//        valve_If2=0;
        //Hand_Data=1;
        //Hand_Data_D=0;
      }

      if (PS3.getButtonPress(L1)){
        valve=1;
        valve2=1;
        valve_If2=1;
      }
      if (PS3.getButtonPress(L2)){
         valve=1;
         valve2=0;
         valve_If2=0;
      }
      if (PS3.getButtonPress(R1)){
         valve=1;
         valve3=1;
         valve_If3=1;
      }
      if (PS3.getButtonPress(R2)){
          valve=1;
          valve3=0;
          valve_If3=0;
      }

      if (PS3.getButtonPress(SELECT)) {
         valve1=0;
         valve_If1=0;
      }
      if (PS3.getButtonPress(START)) {
        valve=1;
        valve1=1;
        valve_If1=1;
      }

      if (PS3.getAnalogHat(RightHatY)){
        pwmR=(PS3.getAnalogHat(RightHatY));
      }

      if (PS3.getAnalogHat(LeftHatY)){
        pwmL=(PS3.getAnalogHat(LeftHatY));
      }

      
       if(valve_If1==0 && valve_If2==0 && valve_If3==0){
        valve=0;
}


  if(Lock == 0){
   Lair=valve;
   Lair1=valve1;
   Lair2=valve2;
   Lair3=valve3;
   Lair_T=valveT;
   Lair_If1=valve_If1;
   Lair_If2=valve_If2;
   Lair_count=valve_count;
    }
  if(Lock==1){
   valve=Lair;
   valve1=Lair1;
   valve2=Lair2;
   valve3=Lair3;
   valveT=Lair_T;
   valve_If1=Lair_If1;
   valve_If2=Lair_If2;
   valve_count=Lair_count;

}


Data=moter;

Data=Data<<1;

Data=Data+valve;

Data=Data<<1;

Data=Data+valve1;

Data=Data<<1;

Data=Data+valve2;

Data=Data<<1;

Data=Data+valve3;

Data=Data<<1;

Data=Data+valveT;





///////////////////
///  2byte目  ////
/////////////////



 Data2=moterR;

 Data2=Data2<<1;

 Data2=Data2+moterL;

 Data2=Data2<<1;

 Data2=Data2+Arm_Data;

 Data2=Data2<<1;

 Data2=Data2+Hand_Data;

 Data2=Data2<<1;

 Data2=Data2+moterR_D;

 Data2=Data2<<1;

 Data2=Data2+moterL_D;

 Data2=Data2<<1;

 Data2=Data2+Arm_Data_D;

 Data2=Data2<<1;
 
 Data2=Data2+Hand_Data_D;

// Serial.write(255);
//
// Serial.write(Data);
//
// Serial.write(Data2);
//
// Serial.write(pwmR);
//
// Serial.write(pwmL);
//
//
// Serial.println(Data,BIN);
//
// Serial.println(Data2,BIN);
//
// Serial.println(pwmR);
//
// Serial.println(pwmL);
//
// Serial.println();
//
//Wire.write(Data);
//
//Wire.write(Data2);
//
//Wire.write(pwmR);
//
//Wire.write(pwmL);





Serial.println(Data,BIN);

Serial.println(Data2,BIN);

Serial.println(pwmR);

Serial.println(pwmL);

Serial.println();


 ////////////////////////////////////
 ///  入力が無いときは初期化  //////
 //////////////////////////////////


 
 moter=0;
 moterR=0;
 moterR_D=0;
 moterL=0;
 moterL_D=0;
 Arm_Data=0;
 Arm_Data_D=0;
 Hand_Data=0;
 Hand_Data_D=0;

    }


※タグ化回避のため、ライブラリ参照時の<>は外しています。



今回やってみた感想としては、実験は念入りにするのが大事ってことですね...

なにせモータードライバの不良品が多かったもので、一番そこで振り回されました(笑)

まあ最終的にはちゃんと動くものができたので良かったと思っています。後輩もちゃんと指示した通りに、時にはそれ以上に動いてくれたので、制作しやすかったです。感謝しています。

動画も撮るつもりだったのですが、調整と競技に夢中でとるのを忘れていました(笑)

 

メール通知機も一緒に載せようと思ったんですが、さすがに長いので次の記事にします

 

それでは次の記事でまたお会いしましょう〜

 

See you next!