File content as of revision 0:c2b9f7662334:

/*プログラム設定
    "モーター番号"    　"ボールセンサ番号"    "ライン・距離センサ番号"
     ___ __ ___          ___ __ ___            ___ __ ___
    /          \        /    b1    \          /    s1    \
    |ch1    ch2|        |b2      b3|          |          |
    |          |        |          |          |s2      s3|
    |ch3    ch4|        |b4      b5|          |          |
    \__________/        \____b6____/          \____s4____/
    
※１ 上方向が前
※２ モーターの正方向は時計回り
※３ 進行方向（ｘ）は正面を０度に時計回りに加算

  動作マニュアル
・ピン番号は要確認
・起動時とリセット時に地磁気基準方向決定 ⇒ 地磁気は前に固定、コートに戻す際注意
・要調整の場所は微調整しておくこと
*/

#include "mbed.h"
#include "Ping.h"
#include "HMC6352.h"
#include "DMdriver.h"
#include "math.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

//使用するピン
AnalogIn ball1(p15), ball2(p16), ball3(p17), ball4(p18), ball5(p19), ball6(p20); //正面,左前,右前,左後,右後,背後 ・要調整
DigitalIn line1(p11), line2(p12), line3(p13), line4(p14); //正面,左,右,背後 ・要調整
DigitalIn toggle(p7); //・要調整
HMC6352 mag(p28 , p27); //(SDA,SCL)・要調整
Driver Driver(p9 , p10); //(SDA,SCL)・要調整
Ping ping1(p22),ping2(p23),ping3(p21),ping4(p24); //正面,左,右,背後 ・要調整

//グローバル変数
int turn = 3; //回転方向判断変数
int motorpower[4] = {-60,60,60,-60}; //モーターのパワー・要調整
float magbasis = -1.0; //地磁気基準値
float magrange = 20.0; //方向判断の誤差 ・要調整
float x = 0.0; //進行方向

//プロトタイプ宣言
void turnDicision(); 
void turnjudge();
void balljudge();
void linejudge();
void move();

int main(){ //メイン
    turnDicision(); //地磁気方向決定
    while(1){
        if(toggle){  //トグルスイッチがオンの時
           turnjudge(); //向き判断            
           if(turn == 0){ //正面を向いているとき
              balljudge(); //ボール判断
              linejudge(); //ライン判断 
           }  
        }else{
            turn = 3; //停止
        }
        move(); //動作
    }
}

void turnDicision(){ //地磁気方向決定
    magbasis = mag.sample()/10.0;
}

void turnjudge(){ //地磁気判断
    
    float magdata = -1.0;
    magdata = mag.sample()/10.0 - magbasis;
      if(magdata < 0.0){
          magdata += 360.0;
      }
    
    if(magrange < magdata && magdata < 180.0){ //30°～180°
       turn = 1;
    }else if(180.0 <= magdata && magdata < 360.0 - magrange){ //180°～330°
       turn = 2;
    }else{ //330°～30°
       turn = 0;
    }
}

void linejudge(){ //ライン・距離センサによる白線処理
     
     int range[4];
     
     ping1.Send(); 
     ping2.Send();  
     ping3.Send();  
     ping4.Send();  
           
     wait_ms(30);
        
     range[0] = ping1.Read_cm(); //正面 
     range[1] = ping2.Read_cm(); //左 
     range[2] = ping3.Read_cm(); //右 
     range[3] = ping4.Read_cm(); //背後
        
     if(line1){ //正面
      if(range[0] > range[3]){
          x = 180.0; //後退
      }else{
          x = 0.0; //前進
      }
    }else if(line2){ //左
      if(range[1] > range[2]){
          x = 90.0; //右へ進む
      }else{
          x = 270.0; //左へ進む
      }
    }else if(line3){ //右
      if(range[1] > range[2]){
          x = 90.0; //右へ進む
      }else{
          x = 270.0; //左へ進む
      }
    }else if(line4){ //背後
      if(range[0] > range[3]){
          x = 180.0; //後退
      }else{
          x = 0.0; //前進
      }
    }else{
    }
}    

void balljudge(){ //ボールの位置判断
    
    int i;
    float ball[6];
    float max = 0.0;//最大値
    float min = 10.0;//最小値
    float GX,GY;

    ball[0] = ball1.read(); //正面 
    ball[1] = ball2.read(); //左前 
    ball[2] = ball3.read(); //右前 
    ball[3] = ball4.read(); //左後 
    ball[4] = ball5.read(); //右後 
    ball[5] = ball6.read(); //背後 
    
    for (i = 0; i < 6; i++){//最大値判断
        if (max < ball[i]){
           max = ball[i];
        }else if (min > ball[i]){
           min = ball[i];
        }
    }
    
    GX = (-ball[1] + ball[2] - ball[3] + ball[4]) * sqrt(3.0)/12.0;
    GY = ((ball[1] + ball[2] - ball[3] - ball[4])/2.0 + ball[0] - ball[5])/6.0;
    
    x = -atan2(-GX, GY)*180.0/M_PI; //ボールの角度算出
    
    if(45.0 < x && x <= 180.0){ //回り込み・要調整
      if(max > 0.6){ //ボールセンサ閾値・要調整
         x += 45.0; //回り込む角度・要調整
      }else{
         x += 20.0; //回り込む角度・要調整
      }
    }else if(180.0 < x && x < 315.0){ //回り込み・要調整
      if(max > 0.6){ //ボールセンサ閾値・要調整
         x -= 45.0; //回り込む角度・要調整
      }else{
         x -= 20.0; //回り込む角度・要調整
      }
    }
    
    if(min < 0.1){ //ボールがないとき・要調整
       x = 180.0; //後退
    }
}

void move(){ //移動・回転方向判断・動作
    
    int mp[4];
    
    switch(turn){
        case 0: //正面   
        mp[0] = sin((x - 315.0)*M_PI/180.0) * motorpower[0];//ch1
        mp[1] = sin((x - 45.0)*M_PI/180.0) * motorpower[1];//ch2
        mp[2] = sin((x - 225.0)*M_PI/180.0) * motorpower[2];//ch3
        mp[3] = sin((x - 135.0)*M_PI/180.0) * motorpower[3];//ch4
        break;
        case 1: //30°～180°
        mp[0] = -motorpower[0]/2;
        mp[1] = -motorpower[1]/2;
        mp[2] = -motorpower[2]/2;
        mp[3] = -motorpower[3]/2;
        break;
        case 2: //180°～330°
        mp[0] = motorpower[0]/2;
        mp[1] = motorpower[1]/2;
        mp[2] = motorpower[2]/2;
        mp[3] = motorpower[3]/2;
        break;
        default: //停止
        mp[0] = 0;
        mp[1] = 0;
        mp[2] = 0;
        mp[3] = 0;
        break;
     }
    Driver.motor(mp[0], mp[1], mp[2], mp[3]);
}