main.cpp

00001 #include "mbed.h"
00002 #include "TB6612.h"
00003 #include "HMC6352.h"
00004 #include "us015.h"
00005 
00006 TB6612      motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
00007 TB6612      motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
00008 Serial      pc(USBTX,USBRX);    //USBシリアル通信用
00009 HMC6352 compass(D4, D5);
00010 US015 hs(D2,D3);
00011 int d;
00012 int n;
00013 
00014 int motor(char m) {
00015     float   motor_speed;        //モータスピード情報格納用
00016         while(1) {
00017         motor_speed=0.5; //モータスピード(低速運転させるため２分の１の値とする。)
00018         switch(m)
00019         {
00020             case    '1':    motor_a=motor_speed;    //モータA正転
00021                             break;    
00022             case    '2':    motor_a=0;              //モータAブレーキ
00023                             break;
00024             case    '3':    motor_a=-motor_speed;   //モータA逆転
00025                             break;                                        
00026             case    '7':    motor_b=motor_speed;    //モータB正転
00027                             break;    
00028             case    '8':    motor_b=0;              //モータBブレーキ
00029                             break;
00030             case    '9':    motor_b=-motor_speed;   //モータB逆転
00031                             break;              
00032             default    :    motor_a=0;
00033                             motor_b=0;              //両方モータブレーキ
00034                             break;        
00035         }
00036     }
00037 }
00038 
00039 int Compass()
00040 {
00041     int angle;
00042     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00043     angle = compass.sample() / 10;
00044     return angle;
00045 }
00046 
00047 int Sonic()
00048 {
00049     int distance;
00050     distance = hs.GetDistance();
00051     return distance;
00052 }
00053 
00054 int main()
00055 {
00056     motor(1);
00057     motor(7);
00058     while(1)
00059     {
00060         n = Compass();
00061         if(n >= 5) {    //5°ずれると片方のモーターが逆転する
00062         motor(3); 
00063         wait(5.0);
00064         }
00065         else {
00066         motor(1);
00067         motor(7); 
00068         }
00069         
00070         d = Sonic();
00071         if(d<=2000){    //超音波反応
00072         motor(3);  
00073         wait(5.0);
00074         motor(1);
00075         }
00076     }
00077 }