おためしらいぶらり Q-rover-Kaiを50行以内で動かすという強い気持ち 使用非推奨 動作未確認 単純なPWM動作のライブラリ化実験用
Fork of QMotor by
QMotor.cpp@0:48d84c228105, 2017-12-06 (annotated)
- Committer:
- KOTAROYamamoto
- Date:
- Wed Dec 06 07:07:32 2017 +0000
- Revision:
- 0:48d84c228105
- Child:
- 1:b80cd8133481
?????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
KOTAROYamamoto | 0:48d84c228105 | 1 | #include "mbed.h" |
KOTAROYamamoto | 0:48d84c228105 | 2 | #include "QMotor.h" |
KOTAROYamamoto | 0:48d84c228105 | 3 | |
KOTAROYamamoto | 0:48d84c228105 | 4 | QMotor::QMotor(PinName R1,PinName R2,PinName L1,PinName L2):_R1(R1),_R2(R2),_L1(L1),_L2(L2){ |
KOTAROYamamoto | 0:48d84c228105 | 5 | _R1=0.0;//R1を初期化 |
KOTAROYamamoto | 0:48d84c228105 | 6 | _R2=0.0;//R2を初期化 |
KOTAROYamamoto | 0:48d84c228105 | 7 | _L1=0.0;//L1を初期化 |
KOTAROYamamoto | 0:48d84c228105 | 8 | _L2=0.0;//L2を初期化 |
KOTAROYamamoto | 0:48d84c228105 | 9 | |
KOTAROYamamoto | 0:48d84c228105 | 10 | } |
KOTAROYamamoto | 0:48d84c228105 | 11 | |
KOTAROYamamoto | 0:48d84c228105 | 12 | void QMotor::Front1(){ //直進部 |
KOTAROYamamoto | 0:48d84c228105 | 13 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 14 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 15 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 16 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 17 | wait(25.000); |
KOTAROYamamoto | 0:48d84c228105 | 18 | |
KOTAROYamamoto | 0:48d84c228105 | 19 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 20 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 21 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 22 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 23 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 24 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 25 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 26 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 27 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 28 | } |
KOTAROYamamoto | 0:48d84c228105 | 29 | |
KOTAROYamamoto | 0:48d84c228105 | 30 | void QMotor::Front2(){ //直進部 |
KOTAROYamamoto | 0:48d84c228105 | 31 | _R1=0.5; |
KOTAROYamamoto | 0:48d84c228105 | 32 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 33 | _L1=0.5; |
KOTAROYamamoto | 0:48d84c228105 | 34 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 35 | wait(5.000); |
KOTAROYamamoto | 0:48d84c228105 | 36 | |
KOTAROYamamoto | 0:48d84c228105 | 37 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 38 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 39 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 40 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 41 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 42 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 43 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 44 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 45 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 46 | } |
KOTAROYamamoto | 0:48d84c228105 | 47 | void QMotor::Right1(float an,float san){ //第一while関数右折部 |
KOTAROYamamoto | 0:48d84c228105 | 48 | //sdprint(" an="); |
KOTAROYamamoto | 0:48d84c228105 | 49 | //sdprint(an); |
KOTAROYamamoto | 0:48d84c228105 | 50 | //sdprint(" R1 "); |
KOTAROYamamoto | 0:48d84c228105 | 51 | //xbee.printf("COMMAND:Right1\n"); |
KOTAROYamamoto | 0:48d84c228105 | 52 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 53 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 54 | wait(-an*san); |
KOTAROYamamoto | 0:48d84c228105 | 55 | |
KOTAROYamamoto | 0:48d84c228105 | 56 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 57 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 58 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 59 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 60 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 61 | } |
KOTAROYamamoto | 0:48d84c228105 | 62 | |
KOTAROYamamoto | 0:48d84c228105 | 63 | void QMotor::Right2(float an,float san){ //第二while関数右折部 |
KOTAROYamamoto | 0:48d84c228105 | 64 | //sdprint(" an="); |
KOTAROYamamoto | 0:48d84c228105 | 65 | //sdprint(an); |
KOTAROYamamoto | 0:48d84c228105 | 66 | //sdprint(" R2 "); |
KOTAROYamamoto | 0:48d84c228105 | 67 | //xbee.printf("COMMAND:Right2\n"); |
KOTAROYamamoto | 0:48d84c228105 | 68 | //analogWrite(5,76.5);//Lモーターを30%出力で回転 |
KOTAROYamamoto | 0:48d84c228105 | 69 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 70 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 71 | wait(-an*san); |
KOTAROYamamoto | 0:48d84c228105 | 72 | |
KOTAROYamamoto | 0:48d84c228105 | 73 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 74 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 75 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 76 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 77 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 78 | } |
KOTAROYamamoto | 0:48d84c228105 | 79 | |
KOTAROYamamoto | 0:48d84c228105 | 80 | |
KOTAROYamamoto | 0:48d84c228105 | 81 | void QMotor::Left1(float an,float san){ //第一while関数左折部 |
KOTAROYamamoto | 0:48d84c228105 | 82 | //sdprint(" an="); |
KOTAROYamamoto | 0:48d84c228105 | 83 | //sdprint(an); |
KOTAROYamamoto | 0:48d84c228105 | 84 | //sdprint(" L1 "); |
KOTAROYamamoto | 0:48d84c228105 | 85 | //xbee.printf("COMMAND:Left1\n"); |
KOTAROYamamoto | 0:48d84c228105 | 86 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 87 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 88 | wait(an*san); |
KOTAROYamamoto | 0:48d84c228105 | 89 | |
KOTAROYamamoto | 0:48d84c228105 | 90 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 91 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 92 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 93 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 94 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 95 | } |
KOTAROYamamoto | 0:48d84c228105 | 96 | |
KOTAROYamamoto | 0:48d84c228105 | 97 | void QMotor::Left2(float an,float san){ //第二while関数左折部 |
KOTAROYamamoto | 0:48d84c228105 | 98 | //sdprint(" an="); |
KOTAROYamamoto | 0:48d84c228105 | 99 | //sdprint(an); |
KOTAROYamamoto | 0:48d84c228105 | 100 | //sdprint(" L2 "); |
KOTAROYamamoto | 0:48d84c228105 | 101 | //xbee.printf("COMMAND:Left2\n"); |
KOTAROYamamoto | 0:48d84c228105 | 102 | //analogWrite(6,76.5);//Rモーターを30%出力で回転 |
KOTAROYamamoto | 0:48d84c228105 | 103 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 104 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 105 | wait(an*san); |
KOTAROYamamoto | 0:48d84c228105 | 106 | |
KOTAROYamamoto | 0:48d84c228105 | 107 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 108 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 109 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 110 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 111 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 112 | } |
KOTAROYamamoto | 0:48d84c228105 | 113 | |
KOTAROYamamoto | 0:48d84c228105 | 114 | void QMotor::Reverse(){ //姿勢回復機動 |
KOTAROYamamoto | 0:48d84c228105 | 115 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 116 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 117 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 118 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 119 | wait(15.000); |
KOTAROYamamoto | 0:48d84c228105 | 120 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 121 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 122 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 123 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 124 | wait(5.000); |
KOTAROYamamoto | 0:48d84c228105 | 125 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 126 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 127 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 128 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 129 | } |
KOTAROYamamoto | 0:48d84c228105 | 130 | |
KOTAROYamamoto | 0:48d84c228105 | 131 | void QMotor::Stack(){ //スタック回避挙動 |
KOTAROYamamoto | 0:48d84c228105 | 132 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 133 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 134 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 135 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 136 | wait(5.000); |
KOTAROYamamoto | 0:48d84c228105 | 137 | |
KOTAROYamamoto | 0:48d84c228105 | 138 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 139 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 140 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 141 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 142 | wait(5.000); |
KOTAROYamamoto | 0:48d84c228105 | 143 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 144 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 145 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 146 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 147 | } |
KOTAROYamamoto | 0:48d84c228105 | 148 | |
KOTAROYamamoto | 0:48d84c228105 | 149 | void QMotor::Fred(){ //冗長系測角用直進部 |
KOTAROYamamoto | 0:48d84c228105 | 150 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 151 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 152 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 153 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 154 | wait(10.000); |
KOTAROYamamoto | 0:48d84c228105 | 155 | |
KOTAROYamamoto | 0:48d84c228105 | 156 | _R1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 157 | _R2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 158 | _L1=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 159 | _L2=1.0; |
KOTAROYamamoto | 0:48d84c228105 | 160 | wait(0.200); |
KOTAROYamamoto | 0:48d84c228105 | 161 | _R1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 162 | _R2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 163 | _L1=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 164 | _L2=0.0; |
KOTAROYamamoto | 0:48d84c228105 | 165 | } |
KOTAROYamamoto | 0:48d84c228105 | 166 |