olol
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
main.cpp@0:29c024d6882f, 2018-09-07 (annotated)
- Committer:
- aoikoizumi
- Date:
- Fri Sep 07 06:32:23 2018 +0000
- Revision:
- 0:29c024d6882f
- Child:
- 1:1817e9243a6e
jk
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aoikoizumi | 0:29c024d6882f | 1 | |
aoikoizumi | 0:29c024d6882f | 2 | #include "mbed.h" |
aoikoizumi | 0:29c024d6882f | 3 | #include "SpeedController.h" |
aoikoizumi | 0:29c024d6882f | 4 | #include "EC.h" |
aoikoizumi | 0:29c024d6882f | 5 | #include "R1370P.h" |
aoikoizumi | 0:29c024d6882f | 6 | #include "enc_1multi.h" |
aoikoizumi | 0:29c024d6882f | 7 | #define BASIC_SPEED 30 //モーターはこの角速度で駆動させる |
aoikoizumi | 0:29c024d6882f | 8 | |
aoikoizumi | 0:29c024d6882f | 9 | SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue |
aoikoizumi | 0:29c024d6882f | 10 | SpeedControl motorL(PB_3,PB_5,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok |
aoikoizumi | 0:29c024d6882f | 11 | Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc |
aoikoizumi | 0:29c024d6882f | 12 | Ticker motor_tick; //角速度計算用ticker |
aoikoizumi | 0:29c024d6882f | 13 | Ticker ticker;//for enc |
aoikoizumi | 0:29c024d6882f | 14 | |
aoikoizumi | 0:29c024d6882f | 15 | Serial pc(USBTX, USBRX); // tx, rx //PC USB |
aoikoizumi | 0:29c024d6882f | 16 | R1370P gyro(PC_6,PC_7); |
aoikoizumi | 0:29c024d6882f | 17 | |
aoikoizumi | 0:29c024d6882f | 18 | |
aoikoizumi | 0:29c024d6882f | 19 | void calOmega() //角速度計算関数 |
aoikoizumi | 0:29c024d6882f | 20 | { |
aoikoizumi | 0:29c024d6882f | 21 | motorR.CalOmega(); |
aoikoizumi | 0:29c024d6882f | 22 | motorL.CalOmega(); |
aoikoizumi | 0:29c024d6882f | 23 | EC1.CalOmega(); |
aoikoizumi | 0:29c024d6882f | 24 | } |
aoikoizumi | 0:29c024d6882f | 25 | |
aoikoizumi | 0:29c024d6882f | 26 | DigitalIn button(USER_BUTTON); |
aoikoizumi | 0:29c024d6882f | 27 | |
aoikoizumi | 0:29c024d6882f | 28 | PwmOut servo(PB_14);//servo |
aoikoizumi | 0:29c024d6882f | 29 | PwmOut motor_f(PC_9); |
aoikoizumi | 0:29c024d6882f | 30 | PwmOut motor_b(PB_9);//arm |
aoikoizumi | 0:29c024d6882f | 31 | DigitalOut denjiben(PC_0);//dennjibenn |
aoikoizumi | 0:29c024d6882f | 32 | |
aoikoizumi | 0:29c024d6882f | 33 | |
aoikoizumi | 0:29c024d6882f | 34 | double new_dist=0; |
aoikoizumi | 0:29c024d6882f | 35 | double old_dist=0; |
aoikoizumi | 0:29c024d6882f | 36 | double d_dist=0; |
aoikoizumi | 0:29c024d6882f | 37 | double x=250; |
aoikoizumi | 0:29c024d6882f | 38 | double y=150;//start point |
aoikoizumi | 0:29c024d6882f | 39 | Timer t; |
aoikoizumi | 0:29c024d6882f | 40 | int i=0; |
aoikoizumi | 0:29c024d6882f | 41 | |
aoikoizumi | 0:29c024d6882f | 42 | int kai=0;//printf kansuu |
aoikoizumi | 0:29c024d6882f | 43 | double target_R=0,target_L=0; |
aoikoizumi | 0:29c024d6882f | 44 | |
aoikoizumi | 0:29c024d6882f | 45 | |
aoikoizumi | 0:29c024d6882f | 46 | double angle; //変数宣言 |
aoikoizumi | 0:29c024d6882f | 47 | |
aoikoizumi | 0:29c024d6882f | 48 | |
aoikoizumi | 0:29c024d6882f | 49 | void tgt(double r,double l) |
aoikoizumi | 0:29c024d6882f | 50 | { |
aoikoizumi | 0:29c024d6882f | 51 | target_R=BASIC_SPEED*r; |
aoikoizumi | 0:29c024d6882f | 52 | target_L=BASIC_SPEED*l; |
aoikoizumi | 0:29c024d6882f | 53 | } |
aoikoizumi | 0:29c024d6882f | 54 | |
aoikoizumi | 0:29c024d6882f | 55 | |
aoikoizumi | 0:29c024d6882f | 56 | void susumu_y(double ay,double by,double goaly) |
aoikoizumi | 0:29c024d6882f | 57 | { |
aoikoizumi | 0:29c024d6882f | 58 | |
aoikoizumi | 0:29c024d6882f | 59 | if(y<goaly-50&&ay>=0) { |
aoikoizumi | 0:29c024d6882f | 60 | t.start(); |
aoikoizumi | 0:29c024d6882f | 61 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 62 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 63 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 64 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 65 | } |
aoikoizumi | 0:29c024d6882f | 66 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 67 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 68 | tgt(ay*2/3,by*2/3); |
aoikoizumi | 0:29c024d6882f | 69 | } else { |
aoikoizumi | 0:29c024d6882f | 70 | tgt(ay,by); |
aoikoizumi | 0:29c024d6882f | 71 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 72 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 73 | } |
aoikoizumi | 0:29c024d6882f | 74 | } |
aoikoizumi | 0:29c024d6882f | 75 | if(y<goaly&&y>=goaly-50&&ay>=0) { |
aoikoizumi | 0:29c024d6882f | 76 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 77 | |
aoikoizumi | 0:29c024d6882f | 78 | |
aoikoizumi | 0:29c024d6882f | 79 | } |
aoikoizumi | 0:29c024d6882f | 80 | if(y>goaly+50&&ay<0) { |
aoikoizumi | 0:29c024d6882f | 81 | t.start(); |
aoikoizumi | 0:29c024d6882f | 82 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 83 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 84 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 85 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 86 | } |
aoikoizumi | 0:29c024d6882f | 87 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 88 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 89 | tgt(ay*2/3,by*2/3); |
aoikoizumi | 0:29c024d6882f | 90 | } else { |
aoikoizumi | 0:29c024d6882f | 91 | tgt(ay,by); |
aoikoizumi | 0:29c024d6882f | 92 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 93 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 94 | } |
aoikoizumi | 0:29c024d6882f | 95 | } |
aoikoizumi | 0:29c024d6882f | 96 | if(y>goaly&&y<=goaly+50&&ay<0) { |
aoikoizumi | 0:29c024d6882f | 97 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 98 | } else { |
aoikoizumi | 0:29c024d6882f | 99 | i++; |
aoikoizumi | 0:29c024d6882f | 100 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 101 | pc.printf("owari\r\n"); |
aoikoizumi | 0:29c024d6882f | 102 | } |
aoikoizumi | 0:29c024d6882f | 103 | } |
aoikoizumi | 0:29c024d6882f | 104 | |
aoikoizumi | 0:29c024d6882f | 105 | |
aoikoizumi | 0:29c024d6882f | 106 | |
aoikoizumi | 0:29c024d6882f | 107 | void susumu_x(double ax,double bx,double goalx) |
aoikoizumi | 0:29c024d6882f | 108 | { |
aoikoizumi | 0:29c024d6882f | 109 | |
aoikoizumi | 0:29c024d6882f | 110 | if(x<goalx-50&&ax>=0) { |
aoikoizumi | 0:29c024d6882f | 111 | t.start(); |
aoikoizumi | 0:29c024d6882f | 112 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 113 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 114 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 115 | tgt(ax/3,bx/3); |
aoikoizumi | 0:29c024d6882f | 116 | } |
aoikoizumi | 0:29c024d6882f | 117 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 118 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 119 | tgt(ax*2/3,bx*2/3); |
aoikoizumi | 0:29c024d6882f | 120 | } else { |
aoikoizumi | 0:29c024d6882f | 121 | tgt(ax,bx); |
aoikoizumi | 0:29c024d6882f | 122 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 123 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 124 | } |
aoikoizumi | 0:29c024d6882f | 125 | } |
aoikoizumi | 0:29c024d6882f | 126 | if(x<goalx&&x>=goalx-50&&ax>=0) { |
aoikoizumi | 0:29c024d6882f | 127 | tgt(ax/3,bx/3); |
aoikoizumi | 0:29c024d6882f | 128 | |
aoikoizumi | 0:29c024d6882f | 129 | } |
aoikoizumi | 0:29c024d6882f | 130 | if(x>goalx+50&&ax<0) { |
aoikoizumi | 0:29c024d6882f | 131 | t.start(); |
aoikoizumi | 0:29c024d6882f | 132 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 133 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 134 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 135 | tgt(ax/3,bx/3); |
aoikoizumi | 0:29c024d6882f | 136 | } |
aoikoizumi | 0:29c024d6882f | 137 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 138 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 139 | tgt(ax*2/3,bx*2/3); |
aoikoizumi | 0:29c024d6882f | 140 | } else { |
aoikoizumi | 0:29c024d6882f | 141 | tgt(ax,bx); |
aoikoizumi | 0:29c024d6882f | 142 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 143 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 144 | } |
aoikoizumi | 0:29c024d6882f | 145 | } |
aoikoizumi | 0:29c024d6882f | 146 | if(x>goalx&&x<=goalx+50&&ax<0) { |
aoikoizumi | 0:29c024d6882f | 147 | tgt(ax/3,bx/3); |
aoikoizumi | 0:29c024d6882f | 148 | |
aoikoizumi | 0:29c024d6882f | 149 | } else { |
aoikoizumi | 0:29c024d6882f | 150 | i++; |
aoikoizumi | 0:29c024d6882f | 151 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 152 | pc.printf("owari\r\n"); |
aoikoizumi | 0:29c024d6882f | 153 | } |
aoikoizumi | 0:29c024d6882f | 154 | } |
aoikoizumi | 0:29c024d6882f | 155 | |
aoikoizumi | 0:29c024d6882f | 156 | |
aoikoizumi | 0:29c024d6882f | 157 | void susumu_ang(double a,double b,double goal) |
aoikoizumi | 0:29c024d6882f | 158 | { |
aoikoizumi | 0:29c024d6882f | 159 | if(x<angle-5&&a>b) {//usetsu |
aoikoizumi | 0:29c024d6882f | 160 | t.start(); |
aoikoizumi | 0:29c024d6882f | 161 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 162 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 163 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 164 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 165 | } |
aoikoizumi | 0:29c024d6882f | 166 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 167 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 168 | tgt(a*2/3,b*2/3); |
aoikoizumi | 0:29c024d6882f | 169 | } else { |
aoikoizumi | 0:29c024d6882f | 170 | tgt(a,b); |
aoikoizumi | 0:29c024d6882f | 171 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 172 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 173 | } |
aoikoizumi | 0:29c024d6882f | 174 | } |
aoikoizumi | 0:29c024d6882f | 175 | if(angle<goal&&angle>=goal-5&&a>b) { |
aoikoizumi | 0:29c024d6882f | 176 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 177 | |
aoikoizumi | 0:29c024d6882f | 178 | } |
aoikoizumi | 0:29c024d6882f | 179 | if(angle>goal+5&&a<b) {//sasetsu |
aoikoizumi | 0:29c024d6882f | 180 | t.start(); |
aoikoizumi | 0:29c024d6882f | 181 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 182 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 183 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 184 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 185 | } |
aoikoizumi | 0:29c024d6882f | 186 | if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 187 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 188 | tgt(a*2/3,b*2/3); |
aoikoizumi | 0:29c024d6882f | 189 | } else { |
aoikoizumi | 0:29c024d6882f | 190 | tgt(a,b); |
aoikoizumi | 0:29c024d6882f | 191 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 192 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 193 | } |
aoikoizumi | 0:29c024d6882f | 194 | } |
aoikoizumi | 0:29c024d6882f | 195 | if(angle>goal&&angle<=goal+5&&a<b) { |
aoikoizumi | 0:29c024d6882f | 196 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 197 | |
aoikoizumi | 0:29c024d6882f | 198 | } else { |
aoikoizumi | 0:29c024d6882f | 199 | i++; |
aoikoizumi | 0:29c024d6882f | 200 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 201 | pc.printf("reset\r\n"); |
aoikoizumi | 0:29c024d6882f | 202 | } |
aoikoizumi | 0:29c024d6882f | 203 | } |
aoikoizumi | 0:29c024d6882f | 204 | |
aoikoizumi | 0:29c024d6882f | 205 | |
aoikoizumi | 0:29c024d6882f | 206 | |
aoikoizumi | 0:29c024d6882f | 207 | |
aoikoizumi | 0:29c024d6882f | 208 | |
aoikoizumi | 0:29c024d6882f | 209 | int main(void) |
aoikoizumi | 0:29c024d6882f | 210 | { |
aoikoizumi | 0:29c024d6882f | 211 | printf("start\r\n"); |
aoikoizumi | 0:29c024d6882f | 212 | motor_tick.attach(&calOmega,0.05); |
aoikoizumi | 0:29c024d6882f | 213 | motorR.setDOconstant(34.1); |
aoikoizumi | 0:29c024d6882f | 214 | motorL.setDOconstant(30);//c |
aoikoizumi | 0:29c024d6882f | 215 | motorR.setPDparam(0,0); |
aoikoizumi | 0:29c024d6882f | 216 | motorR.setPDparam(0,0);//pd |
aoikoizumi | 0:29c024d6882f | 217 | |
aoikoizumi | 0:29c024d6882f | 218 | |
aoikoizumi | 0:29c024d6882f | 219 | gyro.initialize(); //main関数の最初に一度だけ実行 |
aoikoizumi | 0:29c024d6882f | 220 | EC1.setDiameter_mm(50);//sokuteirinnhannkei |
aoikoizumi | 0:29c024d6882f | 221 | double getDistance_mm(); |
aoikoizumi | 0:29c024d6882f | 222 | //int EC1.getCount()=0; |
aoikoizumi | 0:29c024d6882f | 223 | EC1.reset(); |
aoikoizumi | 0:29c024d6882f | 224 | void reset (); |
aoikoizumi | 0:29c024d6882f | 225 | |
aoikoizumi | 0:29c024d6882f | 226 | |
aoikoizumi | 0:29c024d6882f | 227 | // servo.period_ms(60); |
aoikoizumi | 0:29c024d6882f | 228 | |
aoikoizumi | 0:29c024d6882f | 229 | motor_f.period_ms(30); |
aoikoizumi | 0:29c024d6882f | 230 | motor_b.period_ms(30);//arm |
aoikoizumi | 0:29c024d6882f | 231 | |
aoikoizumi | 0:29c024d6882f | 232 | while(1) { |
aoikoizumi | 0:29c024d6882f | 233 | // motorR.turnF(0.3); |
aoikoizumi | 0:29c024d6882f | 234 | //motorL.turnF(0.3);//for debug |
aoikoizumi | 0:29c024d6882f | 235 | motorR.Sc(target_R); |
aoikoizumi | 0:29c024d6882f | 236 | motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う |
aoikoizumi | 0:29c024d6882f | 237 | angle=gyro.getAngle(); //角度の値を受け取る |
aoikoizumi | 0:29c024d6882f | 238 | |
aoikoizumi | 0:29c024d6882f | 239 | EC1.getDistance_mm(); |
aoikoizumi | 0:29c024d6882f | 240 | // EC1.CalOmega(); |
aoikoizumi | 0:29c024d6882f | 241 | |
aoikoizumi | 0:29c024d6882f | 242 | |
aoikoizumi | 0:29c024d6882f | 243 | if(target_R==0) motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 244 | else motorR.Sc(target_R); |
aoikoizumi | 0:29c024d6882f | 245 | if(target_L==0) motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 246 | else motorL.Sc(target_L); |
aoikoizumi | 0:29c024d6882f | 247 | |
aoikoizumi | 0:29c024d6882f | 248 | double d_x=d_dist*sin(angle); |
aoikoizumi | 0:29c024d6882f | 249 | double d_y=d_dist*cos(angle); |
aoikoizumi | 0:29c024d6882f | 250 | x=x+d_x; |
aoikoizumi | 0:29c024d6882f | 251 | y=y+d_y; |
aoikoizumi | 0:29c024d6882f | 252 | |
aoikoizumi | 0:29c024d6882f | 253 | |
aoikoizumi | 0:29c024d6882f | 254 | |
aoikoizumi | 0:29c024d6882f | 255 | if(kai>=3) { |
aoikoizumi | 0:29c024d6882f | 256 | new_dist=EC1.getDistance_mm(); |
aoikoizumi | 0:29c024d6882f | 257 | d_dist=new_dist-old_dist; |
aoikoizumi | 0:29c024d6882f | 258 | old_dist=new_dist; |
aoikoizumi | 0:29c024d6882f | 259 | |
aoikoizumi | 0:29c024d6882f | 260 | //double d_x=d_dist*sin(angle);; |
aoikoizumi | 0:29c024d6882f | 261 | //double d_y=d_dist*cos(angle);; |
aoikoizumi | 0:29c024d6882f | 262 | //x=x+d_x; |
aoikoizumi | 0:29c024d6882f | 263 | //y=y+d_y; |
aoikoizumi | 0:29c024d6882f | 264 | |
aoikoizumi | 0:29c024d6882f | 265 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 266 | // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega()); |
aoikoizumi | 0:29c024d6882f | 267 | pc.printf("i=%d\r\n",i); |
aoikoizumi | 0:29c024d6882f | 268 | //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); |
aoikoizumi | 0:29c024d6882f | 269 | pc.printf("x=%f y=%f",x,y); |
aoikoizumi | 0:29c024d6882f | 270 | //pc.printf("angle=%f\r\n",angle); |
aoikoizumi | 0:29c024d6882f | 271 | //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 |
aoikoizumi | 0:29c024d6882f | 272 | kai=0; |
aoikoizumi | 0:29c024d6882f | 273 | } |
aoikoizumi | 0:29c024d6882f | 274 | kai++; |
aoikoizumi | 0:29c024d6882f | 275 | if(i==0) { |
aoikoizumi | 0:29c024d6882f | 276 | t.start(); |
aoikoizumi | 0:29c024d6882f | 277 | if(t<1) { |
aoikoizumi | 0:29c024d6882f | 278 | motor_f=0.8; |
aoikoizumi | 0:29c024d6882f | 279 | motor_b=0; |
aoikoizumi | 0:29c024d6882f | 280 | } else { |
aoikoizumi | 0:29c024d6882f | 281 | motor_f=0; |
aoikoizumi | 0:29c024d6882f | 282 | motor_b=0; |
aoikoizumi | 0:29c024d6882f | 283 | printf("finish"); |
aoikoizumi | 0:29c024d6882f | 284 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 285 | i++; |
aoikoizumi | 0:29c024d6882f | 286 | } |
aoikoizumi | 0:29c024d6882f | 287 | }//motor test |
aoikoizumi | 0:29c024d6882f | 288 | |
aoikoizumi | 0:29c024d6882f | 289 | //kokomade kihonnsettei |
aoikoizumi | 0:29c024d6882f | 290 | |
aoikoizumi | 0:29c024d6882f | 291 | |
aoikoizumi | 0:29c024d6882f | 292 | /* if(i==0) { |
aoikoizumi | 0:29c024d6882f | 293 | printf("a\n\r"); |
aoikoizumi | 0:29c024d6882f | 294 | target_R=0; |
aoikoizumi | 0:29c024d6882f | 295 | target_L=0; |
aoikoizumi | 0:29c024d6882f | 296 | i++; |
aoikoizumi | 0:29c024d6882f | 297 | pc.printf("i=0\r\n"); |
aoikoizumi | 0:29c024d6882f | 298 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 299 | }*/ |
aoikoizumi | 0:29c024d6882f | 300 | if(i==1) { |
aoikoizumi | 0:29c024d6882f | 301 | susumu_y(1,1,472); |
aoikoizumi | 0:29c024d6882f | 302 | /* if(y<472) { |
aoikoizumi | 0:29c024d6882f | 303 | t.start(); |
aoikoizumi | 0:29c024d6882f | 304 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 305 | if(t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 306 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 307 | target_R=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 308 | target_L=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 309 | } else { |
aoikoizumi | 0:29c024d6882f | 310 | target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 311 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 312 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 313 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 314 | }//250150 250472 |
aoikoizumi | 0:29c024d6882f | 315 | } else { |
aoikoizumi | 0:29c024d6882f | 316 | i++; |
aoikoizumi | 0:29c024d6882f | 317 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 318 | pc.printf("reset\r\n"); |
aoikoizumi | 0:29c024d6882f | 319 | */ //} |
aoikoizumi | 0:29c024d6882f | 320 | } |
aoikoizumi | 0:29c024d6882f | 321 | if(i==2) { |
aoikoizumi | 0:29c024d6882f | 322 | susumu_ang(1/3,1,45); |
aoikoizumi | 0:29c024d6882f | 323 | /* t.start(); |
aoikoizumi | 0:29c024d6882f | 324 | if(angle<45) { |
aoikoizumi | 0:29c024d6882f | 325 | if(t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 326 | target_R=BASIC_SPEED/6; |
aoikoizumi | 0:29c024d6882f | 327 | target_L=BASIC_SPEED/2; |
aoikoizumi | 0:29c024d6882f | 328 | } else { |
aoikoizumi | 0:29c024d6882f | 329 | target_R=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 330 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 331 | }//250472 341691 |
aoikoizumi | 0:29c024d6882f | 332 | } else i++; |
aoikoizumi | 0:29c024d6882f | 333 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 334 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 335 | */ |
aoikoizumi | 0:29c024d6882f | 336 | }//kairyoutyuu |
aoikoizumi | 0:29c024d6882f | 337 | if(i==3) { |
aoikoizumi | 0:29c024d6882f | 338 | susumu_x(1,1,709); |
aoikoizumi | 0:29c024d6882f | 339 | // if(x<709) { |
aoikoizumi | 0:29c024d6882f | 340 | // target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 341 | // target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 342 | // //341691 7091059 |
aoikoizumi | 0:29c024d6882f | 343 | // } else i++; |
aoikoizumi | 0:29c024d6882f | 344 | } |
aoikoizumi | 0:29c024d6882f | 345 | if(i==4) { |
aoikoizumi | 0:29c024d6882f | 346 | susumu_ang(1,1/3,0); |
aoikoizumi | 0:29c024d6882f | 347 | /* if(angle>0) { |
aoikoizumi | 0:29c024d6882f | 348 | target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 349 | target_L=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 350 | //7091059 8001278 |
aoikoizumi | 0:29c024d6882f | 351 | } else i++; |
aoikoizumi | 0:29c024d6882f | 352 | */ |
aoikoizumi | 0:29c024d6882f | 353 | } |
aoikoizumi | 0:29c024d6882f | 354 | if(i==5) { |
aoikoizumi | 0:29c024d6882f | 355 | susumu_y(1,1,1700); |
aoikoizumi | 0:29c024d6882f | 356 | // if(y<1700) { |
aoikoizumi | 0:29c024d6882f | 357 | // target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 358 | // target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 359 | // //8001278 8001700 |
aoikoizumi | 0:29c024d6882f | 360 | // } else i++; |
aoikoizumi | 0:29c024d6882f | 361 | motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 362 | motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 363 | } |
aoikoizumi | 0:29c024d6882f | 364 | |
aoikoizumi | 0:29c024d6882f | 365 | if(i==6) { |
aoikoizumi | 0:29c024d6882f | 366 | |
aoikoizumi | 0:29c024d6882f | 367 | i++; |
aoikoizumi | 0:29c024d6882f | 368 | }//gatiasari |
aoikoizumi | 0:29c024d6882f | 369 | |
aoikoizumi | 0:29c024d6882f | 370 | |
aoikoizumi | 0:29c024d6882f | 371 | /* if(i==7) { |
aoikoizumi | 0:29c024d6882f | 372 | if(angle>=-89) { |
aoikoizumi | 0:29c024d6882f | 373 | target_R=BASIC_SPEED/4; |
aoikoizumi | 0:29c024d6882f | 374 | target_L=BASIC_SPEED*(-4); |
aoikoizumi | 0:29c024d6882f | 375 | } |
aoikoizumi | 0:29c024d6882f | 376 | if(angle<=-91) { |
aoikoizumi | 0:29c024d6882f | 377 | target_R=BASIC_SPEED/4; |
aoikoizumi | 0:29c024d6882f | 378 | target_L=BASIC_SPEED*(-4 |
aoikoizumi | 0:29c024d6882f | 379 | ); |
aoikoizumi | 0:29c024d6882f | 380 | } |
aoikoizumi | 0:29c024d6882f | 381 | if(angle>-91&angle<89){ |
aoikoizumi | 0:29c024d6882f | 382 | motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 383 | motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 384 | wait(0.5); |
aoikoizumi | 0:29c024d6882f | 385 | if(angle>-91&angle<89){ |
aoikoizumi | 0:29c024d6882f | 386 | i++;} |
aoikoizumi | 0:29c024d6882f | 387 | } |
aoikoizumi | 0:29c024d6882f | 388 | } |
aoikoizumi | 0:29c024d6882f | 389 | if(i==8) { |
aoikoizumi | 0:29c024d6882f | 390 | if(x>700) { |
aoikoizumi | 0:29c024d6882f | 391 | target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 392 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 393 | //8001700 7001700 |
aoikoizumi | 0:29c024d6882f | 394 | } else i++; |
aoikoizumi | 0:29c024d6882f | 395 | } |
aoikoizumi | 0:29c024d6882f | 396 | if(i==9) { |
aoikoizumi | 0:29c024d6882f | 397 | if(angle<0) { |
aoikoizumi | 0:29c024d6882f | 398 | target_R=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 399 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 400 | //7001700 4002000 |
aoikoizumi | 0:29c024d6882f | 401 | } else i++; |
aoikoizumi | 0:29c024d6882f | 402 | } |
aoikoizumi | 0:29c024d6882f | 403 | if(i==10) { |
aoikoizumi | 0:29c024d6882f | 404 | if(y<2200) { |
aoikoizumi | 0:29c024d6882f | 405 | target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 406 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 407 | //4002000 4002200 |
aoikoizumi | 0:29c024d6882f | 408 | } else i++; |
aoikoizumi | 0:29c024d6882f | 409 | } |
aoikoizumi | 0:29c024d6882f | 410 | if(i==11) { |
aoikoizumi | 0:29c024d6882f | 411 | if(angle<90) { |
aoikoizumi | 0:29c024d6882f | 412 | target_R=BASIC_SPEED/3; |
aoikoizumi | 0:29c024d6882f | 413 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 414 | //4002200 7002500 |
aoikoizumi | 0:29c024d6882f | 415 | } else i++; |
aoikoizumi | 0:29c024d6882f | 416 | } |
aoikoizumi | 0:29c024d6882f | 417 | if(i==12) { |
aoikoizumi | 0:29c024d6882f | 418 | if(x<1000) { |
aoikoizumi | 0:29c024d6882f | 419 | target_R=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 420 | target_L=BASIC_SPEED; |
aoikoizumi | 0:29c024d6882f | 421 | //7002500 10002500 |
aoikoizumi | 0:29c024d6882f | 422 | } else i++; |
aoikoizumi | 0:29c024d6882f | 423 | motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 424 | motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 425 | } |
aoikoizumi | 0:29c024d6882f | 426 | |
aoikoizumi | 0:29c024d6882f | 427 | if(i==13){ |
aoikoizumi | 0:29c024d6882f | 428 | i++; |
aoikoizumi | 0:29c024d6882f | 429 | } |
aoikoizumi | 0:29c024d6882f | 430 | |
aoikoizumi | 0:29c024d6882f | 431 | |
aoikoizumi | 0:29c024d6882f | 432 | |
aoikoizumi | 0:29c024d6882f | 433 | |
aoikoizumi | 0:29c024d6882f | 434 | if(i==14) { |
aoikoizumi | 0:29c024d6882f | 435 | if(x>700) { |
aoikoizumi | 0:29c024d6882f | 436 | target_R=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 437 | target_L=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 438 | //10002500 7002500 |
aoikoizumi | 0:29c024d6882f | 439 | } else i++; |
aoikoizumi | 0:29c024d6882f | 440 | } |
aoikoizumi | 0:29c024d6882f | 441 | if(i==15) { |
aoikoizumi | 0:29c024d6882f | 442 | if(angle>0) { |
aoikoizumi | 0:29c024d6882f | 443 | target_R=BASIC_SPEED/(-3); |
aoikoizumi | 0:29c024d6882f | 444 | target_L=BASIC_SPEED/(-1); |
aoikoizumi | 0:29c024d6882f | 445 | //7002500 4002200 |
aoikoizumi | 0:29c024d6882f | 446 | } else i++; |
aoikoizumi | 0:29c024d6882f | 447 | } |
aoikoizumi | 0:29c024d6882f | 448 | if(i==16) { |
aoikoizumi | 0:29c024d6882f | 449 | if(y>2000) { |
aoikoizumi | 0:29c024d6882f | 450 | target_R=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 451 | target_L=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 452 | //4002200 4002000 |
aoikoizumi | 0:29c024d6882f | 453 | } else i++; |
aoikoizumi | 0:29c024d6882f | 454 | } |
aoikoizumi | 0:29c024d6882f | 455 | if(i==17) { |
aoikoizumi | 0:29c024d6882f | 456 | if(angle>-90) { |
aoikoizumi | 0:29c024d6882f | 457 | target_R=BASIC_SPEED/(-3); |
aoikoizumi | 0:29c024d6882f | 458 | target_L=BASIC_SPEED/(-1); |
aoikoizumi | 0:29c024d6882f | 459 | //4002000 7001700 |
aoikoizumi | 0:29c024d6882f | 460 | } else i++; |
aoikoizumi | 0:29c024d6882f | 461 | } |
aoikoizumi | 0:29c024d6882f | 462 | if(i==16) { |
aoikoizumi | 0:29c024d6882f | 463 | if(x<1100) { |
aoikoizumi | 0:29c024d6882f | 464 | target_R=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 465 | target_L=BASIC_SPEED*(-1); |
aoikoizumi | 0:29c024d6882f | 466 | //7001700 4002000 |
aoikoizumi | 0:29c024d6882f | 467 | } else i++; |
aoikoizumi | 0:29c024d6882f | 468 | } |
aoikoizumi | 0:29c024d6882f | 469 | */ |
aoikoizumi | 0:29c024d6882f | 470 | //kairyoutyuu kokomade |
aoikoizumi | 0:29c024d6882f | 471 | |
aoikoizumi | 0:29c024d6882f | 472 | }//while |
aoikoizumi | 0:29c024d6882f | 473 | motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 474 | motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 475 | //LR(0,0); |
aoikoizumi | 0:29c024d6882f | 476 | return 0; |
aoikoizumi | 0:29c024d6882f | 477 | |
aoikoizumi | 0:29c024d6882f | 478 | }//intmain |
aoikoizumi | 0:29c024d6882f | 479 |