おためしらいぶらり Q-rover-Kaiを50行以内で動かすという強い気持ち 使用非推奨 動作未確認 単純なPWM動作のライブラリ化実験用
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Fri Jul 22 2022 04:45:50 by 1.7.2