Kotaro Yamamoto / QMotor

おためしらいぶらり　Q-rover-Kaiを50行以内で動かすという強い気持ち　使用非推奨　動作未確認　単純なPWM動作のライブラリ化実験用

Fork of QMotor by Kotaro Yamamoto

QMotor.cpp@1:b80cd8133481, 2017-12-06 (annotated)

Committer:
KOTAROYamamoto
Date:
Wed Dec 06 23:07:23 2017 +0000
Revision:
1:b80cd8133481
Parent:
0:48d84c228105 
?????????????

Who changed what in which revision?

UserRevisionLine numberNew 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 1:b80cd8133481 9 float san1;
KOTAROYamamoto 1:b80cd8133481 10 float san2;
KOTAROYamamoto 0:48d84c228105 11 }
KOTAROYamamoto 0:48d84c228105 12
KOTAROYamamoto 0:48d84c228105 13 void QMotor::Front1(){ //直進部
KOTAROYamamoto 0:48d84c228105 14 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 15 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 16 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 17 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 18 wait(25.000);
KOTAROYamamoto 0:48d84c228105 19
KOTAROYamamoto 0:48d84c228105 20 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 21 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 22 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 23 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 24 wait(0.200);
KOTAROYamamoto 0:48d84c228105 25 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 26 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 27 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 28 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 29 }
KOTAROYamamoto 0:48d84c228105 30
KOTAROYamamoto 0:48d84c228105 31 void QMotor::Front2(){ //直進部
KOTAROYamamoto 0:48d84c228105 32 _R1=0.5;
KOTAROYamamoto 0:48d84c228105 33 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 34 _L1=0.5;
KOTAROYamamoto 0:48d84c228105 35 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 36 wait(5.000);
KOTAROYamamoto 0:48d84c228105 37
KOTAROYamamoto 0:48d84c228105 38 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 39 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 40 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 41 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 42 wait(0.200);
KOTAROYamamoto 0:48d84c228105 43 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 44 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 45 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 46 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 47 }
KOTAROYamamoto 1:b80cd8133481 48 void QMotor::Right1(float an){ //第一while関数右折部
KOTAROYamamoto 0:48d84c228105 49 //sdprint(" an=");
KOTAROYamamoto 0:48d84c228105 50 //sdprint(an);
KOTAROYamamoto 0:48d84c228105 51 //sdprint(" R1 ");
KOTAROYamamoto 0:48d84c228105 52 //xbee.printf("COMMAND:Right1\n");
KOTAROYamamoto 0:48d84c228105 53 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 54 _R2=0.0;
KOTAROYamamoto 1:b80cd8133481 55 wait(-an*san1);
KOTAROYamamoto 0:48d84c228105 56
KOTAROYamamoto 0:48d84c228105 57 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 58 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 59 wait(0.200);
KOTAROYamamoto 0:48d84c228105 60 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 61 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 62 }
KOTAROYamamoto 0:48d84c228105 63
KOTAROYamamoto 1:b80cd8133481 64 void QMotor::Right2(float an){ //第二while関数右折部
KOTAROYamamoto 0:48d84c228105 65 //sdprint(" an=");
KOTAROYamamoto 0:48d84c228105 66 //sdprint(an);
KOTAROYamamoto 0:48d84c228105 67 //sdprint(" R2 ");
KOTAROYamamoto 0:48d84c228105 68 //xbee.printf("COMMAND:Right2\n");
KOTAROYamamoto 0:48d84c228105 69 //analogWrite(5,76.5);//Lモーターを30%出力で回転
KOTAROYamamoto 0:48d84c228105 70 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 71 _R2=0.0;
KOTAROYamamoto 1:b80cd8133481 72 wait(-an*san2);
KOTAROYamamoto 0:48d84c228105 73
KOTAROYamamoto 0:48d84c228105 74 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 75 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 76 wait(0.200);
KOTAROYamamoto 0:48d84c228105 77 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 78 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 79 }
KOTAROYamamoto 0:48d84c228105 80
KOTAROYamamoto 0:48d84c228105 81
KOTAROYamamoto 1:b80cd8133481 82 void QMotor::Left1(float an){ //第一while関数左折部
KOTAROYamamoto 0:48d84c228105 83 //sdprint(" an=");
KOTAROYamamoto 0:48d84c228105 84 //sdprint(an);
KOTAROYamamoto 0:48d84c228105 85 //sdprint(" L1 ");
KOTAROYamamoto 0:48d84c228105 86 //xbee.printf("COMMAND:Left1\n");
KOTAROYamamoto 0:48d84c228105 87 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 88 _L2=0.0;
KOTAROYamamoto 1:b80cd8133481 89 wait(an*san1);
KOTAROYamamoto 0:48d84c228105 90
KOTAROYamamoto 0:48d84c228105 91 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 92 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 93 wait(0.200);
KOTAROYamamoto 0:48d84c228105 94 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 95 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 96 }
KOTAROYamamoto 0:48d84c228105 97
KOTAROYamamoto 1:b80cd8133481 98 void QMotor::Left2(float an){ //第二while関数左折部
KOTAROYamamoto 0:48d84c228105 99 //sdprint(" an=");
KOTAROYamamoto 0:48d84c228105 100 //sdprint(an);
KOTAROYamamoto 0:48d84c228105 101 //sdprint(" L2 ");
KOTAROYamamoto 0:48d84c228105 102 //xbee.printf("COMMAND:Left2\n");
KOTAROYamamoto 0:48d84c228105 103 //analogWrite(6,76.5);//Rモーターを30%出力で回転
KOTAROYamamoto 0:48d84c228105 104 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 105 _L2=0.0;
KOTAROYamamoto 1:b80cd8133481 106 wait(an*san2);
KOTAROYamamoto 0:48d84c228105 107
KOTAROYamamoto 0:48d84c228105 108 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 109 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 110 wait(0.200);
KOTAROYamamoto 0:48d84c228105 111 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 112 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 113 }
KOTAROYamamoto 0:48d84c228105 114
KOTAROYamamoto 0:48d84c228105 115 void QMotor::Reverse(){ //姿勢回復機動
KOTAROYamamoto 0:48d84c228105 116 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 117 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 118 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 119 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 120 wait(15.000);
KOTAROYamamoto 0:48d84c228105 121 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 122 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 123 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 124 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 125 wait(5.000);
KOTAROYamamoto 0:48d84c228105 126 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 127 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 128 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 129 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 130 }
KOTAROYamamoto 0:48d84c228105 131
KOTAROYamamoto 0:48d84c228105 132 void QMotor::Stack(){ //スタック回避挙動
KOTAROYamamoto 0:48d84c228105 133 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 134 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 135 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 136 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 137 wait(5.000);
KOTAROYamamoto 0:48d84c228105 138
KOTAROYamamoto 0:48d84c228105 139 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 140 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 141 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 142 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 143 wait(5.000);
KOTAROYamamoto 0:48d84c228105 144 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 145 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 146 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 147 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 148 }
KOTAROYamamoto 0:48d84c228105 149
KOTAROYamamoto 0:48d84c228105 150 void QMotor::Fred(){ //冗長系測角用直進部
KOTAROYamamoto 0:48d84c228105 151 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 152 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 153 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 154 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 155 wait(10.000);
KOTAROYamamoto 0:48d84c228105 156
KOTAROYamamoto 0:48d84c228105 157 _R1=1.0;
KOTAROYamamoto 0:48d84c228105 158 _R2=1.0;
KOTAROYamamoto 0:48d84c228105 159 _L1=1.0;
KOTAROYamamoto 0:48d84c228105 160 _L2=1.0;
KOTAROYamamoto 0:48d84c228105 161 wait(0.200);
KOTAROYamamoto 0:48d84c228105 162 _R1=0.0;
KOTAROYamamoto 0:48d84c228105 163 _R2=0.0;
KOTAROYamamoto 0:48d84c228105 164 _L1=0.0;
KOTAROYamamoto 0:48d84c228105 165 _L2=0.0;
KOTAROYamamoto 0:48d84c228105 166 }
KOTAROYamamoto 0:48d84c228105 167