Kotaro Yamamoto / Q-rover-Motor

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

QMotor.cpp@0:48d84c228105, 2017-12-06 (annotated)

Committer:
KOTAROYamamoto
Date:
Wed Dec 06 07:07:32 2017 +0000
Revision:
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 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