QMotor.cpp

00001 #include "mbed.h"
00002 #include "QMotor.h"
00003 
00004 QMotor::QMotor(PinName R1,PinName R2,PinName L1,PinName L2):_R1(R1),_R2(R2),_L1(L1),_L2(L2){
00005     _R1=0.0;//R1を初期化
00006     _R2=0.0;//R2を初期化
00007     _L1=0.0;//L1を初期化
00008     _L2=0.0;//L2を初期化
00009 
00010 }
00011 
00012 void QMotor::Front1(){                   //直進部
00013     _R1=1.0;
00014     _R2=0.0;
00015     _L1=1.0;
00016     _L2=0.0;
00017     wait(25.000);
00018         
00019     _R1=1.0;
00020     _R2=1.0;
00021     _L1=1.0;
00022     _L2=1.0;
00023     wait(0.200);
00024     _R1=0.0;
00025     _R2=0.0;
00026     _L1=0.0;
00027     _L2=0.0;
00028 }
00029 
00030 void QMotor::Front2(){                   //直進部
00031     _R1=0.5;
00032     _R2=0.0;
00033     _L1=0.5;
00034     _L2=0.0;
00035     wait(5.000);
00036         
00037     _R1=1.0;
00038     _R2=1.0;
00039     _L1=1.0;
00040     _L2=1.0;
00041     wait(0.200);
00042     _R1=0.0;
00043     _R2=0.0;
00044     _L1=0.0;
00045     _L2=0.0;
00046 }
00047 void QMotor::Right1(float an,float san){          //第一while関数右折部
00048     //sdprint(" an=");
00049     //sdprint(an);
00050     //sdprint(" R1 ");
00051     //xbee.printf("COMMAND:Right1\n");
00052     _R1=1.0;
00053     _R2=0.0;
00054     wait(-an*san);
00055         
00056     _R1=1.0;
00057     _R2=1.0;
00058     wait(0.200);
00059     _R1=0.0;
00060     _R2=0.0;
00061   }
00062   
00063   void QMotor::Right2(float an,float san){          //第二while関数右折部
00064     //sdprint(" an=");
00065     //sdprint(an);
00066     //sdprint(" R2 ");
00067     //xbee.printf("COMMAND:Right2\n");
00068     //analogWrite(5,76.5);//Lモーターを30%出力で回転
00069     _R1=1.0;
00070     _R2=0.0;
00071     wait(-an*san);
00072         
00073     _R1=1.0;
00074     _R2=1.0;
00075     wait(0.200);
00076     _R1=0.0;
00077     _R2=0.0;
00078   }
00079   
00080   
00081   void QMotor::Left1(float an,float san){           //第一while関数左折部
00082     //sdprint(" an=");
00083     //sdprint(an);
00084     //sdprint(" L1 ");
00085     //xbee.printf("COMMAND:Left1\n");
00086     _L1=1.0;
00087     _L2=0.0;
00088     wait(an*san);
00089         
00090     _L1=1.0;
00091     _L2=1.0;
00092     wait(0.200);
00093     _L1=0.0;
00094     _L2=0.0;
00095   }
00096 
00097   void QMotor::Left2(float an,float san){           //第二while関数左折部
00098     //sdprint(" an=");
00099     //sdprint(an);
00100     //sdprint(" L2 ");
00101     //xbee.printf("COMMAND:Left2\n");
00102     //analogWrite(6,76.5);//Rモーターを30%出力で回転
00103     _L1=1.0;
00104     _L2=0.0;
00105     wait(an*san);
00106         
00107     _L1=1.0;
00108     _L2=1.0;
00109     wait(0.200);
00110     _L1=0.0;
00111     _L2=0.0;      
00112   }
00113   
00114 void QMotor::Reverse(){                 //姿勢回復機動
00115     _R1=0.0;
00116     _R2=1.0;
00117     _L1=0.0;
00118     _L2=1.0;
00119     wait(15.000);
00120     _R1=1.0;
00121     _R2=0.0;
00122     _L1=1.0;
00123     _L2=0.0;
00124     wait(5.000);
00125     _R1=0.0;
00126     _R2=0.0;
00127     _L1=0.0;
00128     _L2=0.0;
00129 }
00130   
00131 void QMotor::Stack(){                   //スタック回避挙動
00132     _R1=1.0;
00133     _R2=0.0;
00134     _L1=0.0;
00135     _L2=1.0;
00136     wait(5.000);
00137 
00138     _R1=1.0;
00139     _R2=1.0;
00140     _L1=1.0;
00141     _L2=1.0;
00142     wait(5.000);
00143     _L1=0.0;
00144     _L2=0.0;
00145     _R1=0.0;
00146     _R2=0.0;
00147   }
00148   
00149     void QMotor::Fred(){                    //冗長系測角用直進部
00150     _R1=1.0;
00151     _R2=0.0;
00152     _L1=1.0;
00153     _L2=0.0;
00154     wait(10.000);
00155       
00156     _R1=1.0;
00157     _R2=1.0;
00158     _L1=1.0;
00159     _L2=1.0;
00160     wait(0.200);
00161     _R1=0.0;
00162     _R2=0.0;
00163     _L1=0.0;
00164     _L2=0.0;
00165   }
00166