![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
F3RC9/15
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of F3RCrere by
main.cpp@6:7ec6c3d3a30a, 2018-09-13 (annotated)
- Committer:
- aoikoizumi
- Date:
- Thu Sep 13 04:17:15 2018 +0000
- Revision:
- 6:7ec6c3d3a30a
- Parent:
- 5:0160b73f3d9e
- Child:
- 7:3a7e49ed1162
9/13 1317
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 | 6:7ec6c3d3a30a | 7 | #define BASIC_SPEED 15 //モーターはこの角速度で駆動させる |
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 | 4:e3d739bf09b4 | 10 | SpeedControl motorL(PB_5,PB_3,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 | 1:1817e9243a6e | 26 | //DigitalIn button(USER_BUTTON); |
aoikoizumi | 1:1817e9243a6e | 27 | DigitalIn reset_f(PC_1,PullUp); |
aoikoizumi | 1:1817e9243a6e | 28 | DigitalIn reset_a(PA_4,PullUp); |
aoikoizumi | 0:29c024d6882f | 29 | |
aoikoizumi | 0:29c024d6882f | 30 | PwmOut servo(PB_14);//servo |
aoikoizumi | 0:29c024d6882f | 31 | PwmOut motor_f(PC_9); |
aoikoizumi | 0:29c024d6882f | 32 | PwmOut motor_b(PB_9);//arm |
aoikoizumi | 0:29c024d6882f | 33 | DigitalOut denjiben(PC_0);//dennjibenn |
aoikoizumi | 0:29c024d6882f | 34 | |
aoikoizumi | 0:29c024d6882f | 35 | |
aoikoizumi | 0:29c024d6882f | 36 | double new_dist=0; |
aoikoizumi | 0:29c024d6882f | 37 | double old_dist=0; |
aoikoizumi | 0:29c024d6882f | 38 | double d_dist=0; |
aoikoizumi | 5:0160b73f3d9e | 39 | double x; |
aoikoizumi | 5:0160b73f3d9e | 40 | double y; |
aoikoizumi | 5:0160b73f3d9e | 41 | double asari_x=900; |
aoikoizumi | 5:0160b73f3d9e | 42 | double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu |
aoikoizumi | 5:0160b73f3d9e | 43 | double goal_x=1120; |
aoikoizumi | 5:0160b73f3d9e | 44 | double goal_y=1700; |
aoikoizumi | 5:0160b73f3d9e | 45 | double start_x=185; |
aoikoizumi | 5:0160b73f3d9e | 46 | double start_y=150; |
aoikoizumi | 0:29c024d6882f | 47 | Timer t; |
aoikoizumi | 4:e3d739bf09b4 | 48 | int i=1; |
aoikoizumi | 0:29c024d6882f | 49 | |
aoikoizumi | 0:29c024d6882f | 50 | int kai=0;//printf kansuu |
aoikoizumi | 0:29c024d6882f | 51 | double target_R=0,target_L=0; |
aoikoizumi | 0:29c024d6882f | 52 | |
aoikoizumi | 0:29c024d6882f | 53 | |
aoikoizumi | 0:29c024d6882f | 54 | double angle; //変数宣言 |
aoikoizumi | 0:29c024d6882f | 55 | |
aoikoizumi | 0:29c024d6882f | 56 | |
aoikoizumi | 1:1817e9243a6e | 57 | void tgt(double r,double l)//void de sika tsukawanaino ha mottainai kimosuru |
aoikoizumi | 0:29c024d6882f | 58 | { |
aoikoizumi | 0:29c024d6882f | 59 | target_R=BASIC_SPEED*r; |
aoikoizumi | 0:29c024d6882f | 60 | target_L=BASIC_SPEED*l; |
aoikoizumi | 0:29c024d6882f | 61 | } |
aoikoizumi | 0:29c024d6882f | 62 | |
aoikoizumi | 0:29c024d6882f | 63 | |
aoikoizumi | 1:1817e9243a6e | 64 | //printf wo dousa kakunin sidai kesou toha omotte imasu |
aoikoizumi | 1:1817e9243a6e | 65 | void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete |
aoikoizumi | 0:29c024d6882f | 66 | { |
aoikoizumi | 1:1817e9243a6e | 67 | //pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly); |
aoikoizumi | 0:29c024d6882f | 68 | if(y<goaly-50&&ay>=0) { |
aoikoizumi | 0:29c024d6882f | 69 | t.start(); |
aoikoizumi | 0:29c024d6882f | 70 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 71 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 72 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 73 | tgt(ay/3,by/3); |
aoikoizumi | 1:1817e9243a6e | 74 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 75 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 76 | tgt(ay*2/3,by*2/3); |
aoikoizumi | 0:29c024d6882f | 77 | } else { |
aoikoizumi | 0:29c024d6882f | 78 | tgt(ay,by); |
aoikoizumi | 0:29c024d6882f | 79 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 80 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 81 | } |
aoikoizumi | 1:1817e9243a6e | 82 | } else if(y<goaly&&y>=goaly-50&&ay>=0) { |
aoikoizumi | 0:29c024d6882f | 83 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 84 | |
aoikoizumi | 0:29c024d6882f | 85 | |
aoikoizumi | 1:1817e9243a6e | 86 | } else if(y>goaly+50&&ay<0) { |
aoikoizumi | 0:29c024d6882f | 87 | t.start(); |
aoikoizumi | 0:29c024d6882f | 88 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 89 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 90 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 91 | tgt(ay/3,by/3); |
aoikoizumi | 1:1817e9243a6e | 92 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 93 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 94 | tgt(ay*2/3,by*2/3); |
aoikoizumi | 0:29c024d6882f | 95 | } else { |
aoikoizumi | 0:29c024d6882f | 96 | tgt(ay,by); |
aoikoizumi | 0:29c024d6882f | 97 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 98 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 99 | } |
aoikoizumi | 1:1817e9243a6e | 100 | } else if(y>goaly&&y<=goaly+50&&ay<0) { |
aoikoizumi | 0:29c024d6882f | 101 | tgt(ay/3,by/3); |
aoikoizumi | 0:29c024d6882f | 102 | } else { |
aoikoizumi | 0:29c024d6882f | 103 | i++; |
aoikoizumi | 0:29c024d6882f | 104 | t.reset(); |
aoikoizumi | 1:1817e9243a6e | 105 | pc.printf("owari%d\r\n",i); |
aoikoizumi | 0:29c024d6882f | 106 | } |
aoikoizumi | 0:29c024d6882f | 107 | } |
aoikoizumi | 0:29c024d6882f | 108 | |
aoikoizumi | 0:29c024d6882f | 109 | |
aoikoizumi | 0:29c024d6882f | 110 | |
aoikoizumi | 1:1817e9243a6e | 111 | void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa |
aoikoizumi | 0:29c024d6882f | 112 | { |
aoikoizumi | 0:29c024d6882f | 113 | |
aoikoizumi | 1:1817e9243a6e | 114 | if(x<goalxl-50) { |
aoikoizumi | 0:29c024d6882f | 115 | t.start(); |
aoikoizumi | 0:29c024d6882f | 116 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 117 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 118 | pc.printf("t=%f",t.read()); |
aoikoizumi | 1:1817e9243a6e | 119 | tgt(axl/3,bxl/3); |
aoikoizumi | 1:1817e9243a6e | 120 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 121 | pc.printf("t=%f",t.read()); |
aoikoizumi | 1:1817e9243a6e | 122 | tgt(axl*2/3,bxl*2/3); |
aoikoizumi | 0:29c024d6882f | 123 | } else { |
aoikoizumi | 1:1817e9243a6e | 124 | tgt(axl,bxl); |
aoikoizumi | 0:29c024d6882f | 125 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 126 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 127 | } |
aoikoizumi | 1:1817e9243a6e | 128 | } else if(x<goalxl&&x>=goalxl-50) { |
aoikoizumi | 1:1817e9243a6e | 129 | tgt(axl/3,bxl/3); |
aoikoizumi | 1:1817e9243a6e | 130 | |
aoikoizumi | 1:1817e9243a6e | 131 | } else { |
aoikoizumi | 1:1817e9243a6e | 132 | t.reset(); |
aoikoizumi | 1:1817e9243a6e | 133 | i++; |
aoikoizumi | 1:1817e9243a6e | 134 | pc.printf("owari\r\n"); |
aoikoizumi | 0:29c024d6882f | 135 | } |
aoikoizumi | 1:1817e9243a6e | 136 | } |
aoikoizumi | 0:29c024d6882f | 137 | |
aoikoizumi | 1:1817e9243a6e | 138 | void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa |
aoikoizumi | 1:1817e9243a6e | 139 | { |
aoikoizumi | 1:1817e9243a6e | 140 | |
aoikoizumi | 1:1817e9243a6e | 141 | if(x>goalxr+50) { |
aoikoizumi | 0:29c024d6882f | 142 | t.start(); |
aoikoizumi | 0:29c024d6882f | 143 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 144 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 145 | pc.printf("t=%f",t.read()); |
aoikoizumi | 1:1817e9243a6e | 146 | tgt(axr/3,bxr/3); |
aoikoizumi | 1:1817e9243a6e | 147 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 148 | pc.printf("t=%f",t.read()); |
aoikoizumi | 1:1817e9243a6e | 149 | tgt(axr*2/3,bxr*2/3); |
aoikoizumi | 0:29c024d6882f | 150 | } else { |
aoikoizumi | 1:1817e9243a6e | 151 | tgt(axr,bxr); |
aoikoizumi | 0:29c024d6882f | 152 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 153 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 154 | } |
aoikoizumi | 1:1817e9243a6e | 155 | } else if(x>goalxr&&x<=goalxr+50) { |
aoikoizumi | 1:1817e9243a6e | 156 | tgt(axr/3,bxr/3); |
aoikoizumi | 0:29c024d6882f | 157 | |
aoikoizumi | 0:29c024d6882f | 158 | } else { |
aoikoizumi | 1:1817e9243a6e | 159 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 160 | i++; |
aoikoizumi | 0:29c024d6882f | 161 | pc.printf("owari\r\n"); |
aoikoizumi | 0:29c024d6882f | 162 | } |
aoikoizumi | 0:29c024d6882f | 163 | } |
aoikoizumi | 0:29c024d6882f | 164 | |
aoikoizumi | 1:1817e9243a6e | 165 | void susumu_ang(double a,double b,double goal)//kakudo |
aoikoizumi | 0:29c024d6882f | 166 | { |
aoikoizumi | 6:7ec6c3d3a30a | 167 | if(goal-5>angle&&a<b) {//usetsu |
aoikoizumi | 0:29c024d6882f | 168 | t.start(); |
aoikoizumi | 0:29c024d6882f | 169 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 170 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 171 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 172 | tgt(a/3,b/3); |
aoikoizumi | 1:1817e9243a6e | 173 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 174 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 175 | tgt(a*2/3,b*2/3); |
aoikoizumi | 0:29c024d6882f | 176 | } else { |
aoikoizumi | 0:29c024d6882f | 177 | tgt(a,b); |
aoikoizumi | 0:29c024d6882f | 178 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 179 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 180 | } |
aoikoizumi | 6:7ec6c3d3a30a | 181 | } else if(angle<goal&&angle>=goal-5&&a<b) { |
aoikoizumi | 0:29c024d6882f | 182 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 183 | |
aoikoizumi | 6:7ec6c3d3a30a | 184 | } else if(angle>goal+5&&a>b) { //sasetsu |
aoikoizumi | 0:29c024d6882f | 185 | t.start(); |
aoikoizumi | 0:29c024d6882f | 186 | pc.printf("R=%f L=%f\r\n",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 187 | if(t.read_ms()<100) { |
aoikoizumi | 0:29c024d6882f | 188 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 189 | tgt(a/3,b/3); |
aoikoizumi | 1:1817e9243a6e | 190 | } else if(t.read_ms()>=100&&t.read_ms()<200) { |
aoikoizumi | 0:29c024d6882f | 191 | pc.printf("t=%f",t.read()); |
aoikoizumi | 0:29c024d6882f | 192 | tgt(a*2/3,b*2/3); |
aoikoizumi | 0:29c024d6882f | 193 | } else { |
aoikoizumi | 0:29c024d6882f | 194 | tgt(a,b); |
aoikoizumi | 0:29c024d6882f | 195 | t.stop(); |
aoikoizumi | 0:29c024d6882f | 196 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 197 | } |
aoikoizumi | 6:7ec6c3d3a30a | 198 | } else if(angle>goal&&angle<=goal+5&&a>b) { |
aoikoizumi | 0:29c024d6882f | 199 | tgt(a/3,b/3); |
aoikoizumi | 0:29c024d6882f | 200 | |
aoikoizumi | 0:29c024d6882f | 201 | } else { |
aoikoizumi | 0:29c024d6882f | 202 | i++; |
aoikoizumi | 0:29c024d6882f | 203 | t.reset(); |
aoikoizumi | 0:29c024d6882f | 204 | pc.printf("reset\r\n"); |
aoikoizumi | 0:29c024d6882f | 205 | } |
aoikoizumi | 0:29c024d6882f | 206 | } |
aoikoizumi | 0:29c024d6882f | 207 | |
aoikoizumi | 0:29c024d6882f | 208 | |
aoikoizumi | 0:29c024d6882f | 209 | |
aoikoizumi | 0:29c024d6882f | 210 | |
aoikoizumi | 0:29c024d6882f | 211 | |
aoikoizumi | 0:29c024d6882f | 212 | int main(void) |
aoikoizumi | 0:29c024d6882f | 213 | { |
aoikoizumi | 5:0160b73f3d9e | 214 | |
aoikoizumi | 5:0160b73f3d9e | 215 | gyro.initialize(); //main関数の最初に一度だけ実行 |
aoikoizumi | 5:0160b73f3d9e | 216 | gyro.acc_offset(); //やってもやらなくてもいい |
aoikoizumi | 5:0160b73f3d9e | 217 | |
aoikoizumi | 0:29c024d6882f | 218 | printf("start\r\n"); |
aoikoizumi | 0:29c024d6882f | 219 | motor_tick.attach(&calOmega,0.05); |
aoikoizumi | 0:29c024d6882f | 220 | motorR.setDOconstant(34.1); |
aoikoizumi | 0:29c024d6882f | 221 | motorL.setDOconstant(30);//c |
aoikoizumi | 0:29c024d6882f | 222 | motorR.setPDparam(0,0); |
aoikoizumi | 1:1817e9243a6e | 223 | motorR.setPDparam(0,0);//pd//akirameta |
aoikoizumi | 0:29c024d6882f | 224 | |
aoikoizumi | 0:29c024d6882f | 225 | |
aoikoizumi | 0:29c024d6882f | 226 | EC1.setDiameter_mm(50);//sokuteirinnhannkei |
aoikoizumi | 0:29c024d6882f | 227 | double getDistance_mm(); |
aoikoizumi | 0:29c024d6882f | 228 | //int EC1.getCount()=0; |
aoikoizumi | 5:0160b73f3d9e | 229 | void reset (); |
aoikoizumi | 0:29c024d6882f | 230 | EC1.reset(); |
aoikoizumi | 0:29c024d6882f | 231 | |
aoikoizumi | 0:29c024d6882f | 232 | |
aoikoizumi | 1:1817e9243a6e | 233 | servo.period_ms(20); |
aoikoizumi | 0:29c024d6882f | 234 | |
aoikoizumi | 0:29c024d6882f | 235 | motor_f.period_ms(30); |
aoikoizumi | 0:29c024d6882f | 236 | motor_b.period_ms(30);//arm |
aoikoizumi | 5:0160b73f3d9e | 237 | x=start_x; |
aoikoizumi | 5:0160b73f3d9e | 238 | y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu |
aoikoizumi | 0:29c024d6882f | 239 | |
aoikoizumi | 0:29c024d6882f | 240 | while(1) { |
aoikoizumi | 0:29c024d6882f | 241 | // motorR.turnF(0.3); |
aoikoizumi | 0:29c024d6882f | 242 | //motorL.turnF(0.3);//for debug |
aoikoizumi | 0:29c024d6882f | 243 | motorR.Sc(target_R); |
aoikoizumi | 0:29c024d6882f | 244 | motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う |
aoikoizumi | 5:0160b73f3d9e | 245 | angle=gyro.getAngle()*(-1); //角度の値を受け取る |
aoikoizumi | 0:29c024d6882f | 246 | |
aoikoizumi | 0:29c024d6882f | 247 | EC1.getDistance_mm(); |
aoikoizumi | 0:29c024d6882f | 248 | // EC1.CalOmega(); |
aoikoizumi | 0:29c024d6882f | 249 | |
aoikoizumi | 0:29c024d6882f | 250 | |
aoikoizumi | 0:29c024d6882f | 251 | if(target_R==0) motorR.stop(); |
aoikoizumi | 0:29c024d6882f | 252 | else motorR.Sc(target_R); |
aoikoizumi | 0:29c024d6882f | 253 | if(target_L==0) motorL.stop(); |
aoikoizumi | 0:29c024d6882f | 254 | else motorL.Sc(target_L); |
aoikoizumi | 6:7ec6c3d3a30a | 255 | |
aoikoizumi | 6:7ec6c3d3a30a | 256 | new_dist=EC1.getDistance_mm(); |
aoikoizumi | 6:7ec6c3d3a30a | 257 | d_dist=new_dist-old_dist; |
aoikoizumi | 6:7ec6c3d3a30a | 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 | |
aoikoizumi | 0:29c024d6882f | 266 | |
aoikoizumi | 0:29c024d6882f | 267 | if(kai>=3) { |
aoikoizumi | 6:7ec6c3d3a30a | 268 | |
aoikoizumi | 0:29c024d6882f | 269 | |
aoikoizumi | 0:29c024d6882f | 270 | //double d_x=d_dist*sin(angle);; |
aoikoizumi | 0:29c024d6882f | 271 | //double d_y=d_dist*cos(angle);; |
aoikoizumi | 0:29c024d6882f | 272 | //x=x+d_x; |
aoikoizumi | 0:29c024d6882f | 273 | //y=y+d_y; |
aoikoizumi | 0:29c024d6882f | 274 | |
aoikoizumi | 0:29c024d6882f | 275 | pc.printf("R=%f L=%f",target_R,target_L); |
aoikoizumi | 0:29c024d6882f | 276 | // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega()); |
aoikoizumi | 0:29c024d6882f | 277 | pc.printf("i=%d\r\n",i); |
aoikoizumi | 0:29c024d6882f | 278 | //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); |
aoikoizumi | 0:29c024d6882f | 279 | pc.printf("x=%f y=%f",x,y); |
aoikoizumi | 4:e3d739bf09b4 | 280 | pc.printf("angle=%f\r\n",angle); |
aoikoizumi | 0:29c024d6882f | 281 | //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 |
aoikoizumi | 0:29c024d6882f | 282 | kai=0; |
aoikoizumi | 0:29c024d6882f | 283 | } |
aoikoizumi | 0:29c024d6882f | 284 | kai++; |
aoikoizumi | 0:29c024d6882f | 285 | if(i==0) { |
aoikoizumi | 4:e3d739bf09b4 | 286 | /*if(reset_f.read()==1) { |
aoikoizumi | 1:1817e9243a6e | 287 | wait(0.05); |
aoikoizumi | 1:1817e9243a6e | 288 | if(reset_f.read()==1) { |
aoikoizumi | 4:e3d739bf09b4 | 289 | */ denjiben=0; |
aoikoizumi | 4:e3d739bf09b4 | 290 | i++; |
aoikoizumi | 4:e3d739bf09b4 | 291 | // } |
aoikoizumi | 4:e3d739bf09b4 | 292 | //} |
aoikoizumi | 4:e3d739bf09b4 | 293 | if(reset_a.read()==1) { |
aoikoizumi | 4:e3d739bf09b4 | 294 | wait(0.05); |
aoikoizumi | 2:3176040a4777 | 295 | if(reset_a.read()==1) { |
aoikoizumi | 4:e3d739bf09b4 | 296 | denjiben=0; |
aoikoizumi | 4:e3d739bf09b4 | 297 | x=70; |
aoikoizumi | 4:e3d739bf09b4 | 298 | y=2500; |
aoikoizumi | 4:e3d739bf09b4 | 299 | i=14; |
aoikoizumi | 2:3176040a4777 | 300 | } |
aoikoizumi | 5:0160b73f3d9e | 301 | }//x,y |
aoikoizumi | 4:e3d739bf09b4 | 302 | } |
aoikoizumi | 4:e3d739bf09b4 | 303 | if(i==1) { |
aoikoizumi | 5:0160b73f3d9e | 304 | susumu_y(1,1,start_x+178);//x,x+178 |
aoikoizumi | 4:e3d739bf09b4 | 305 | } |
aoikoizumi | 4:e3d739bf09b4 | 306 | if(i==2) { |
aoikoizumi | 6:7ec6c3d3a30a | 307 | susumu_ang(0.5,1,45);//x+121.5,x+471.5 |
aoikoizumi | 4:e3d739bf09b4 | 308 | } |
aoikoizumi | 4:e3d739bf09b4 | 309 | if(i==3) { |
aoikoizumi | 5:0160b73f3d9e | 310 | susumu_xl(1,1,728.5);//728.5,1078.5 |
aoikoizumi | 4:e3d739bf09b4 | 311 | } |
aoikoizumi | 4:e3d739bf09b4 | 312 | if(i==4) { |
aoikoizumi | 6:7ec6c3d3a30a | 313 | susumu_ang(1,0.5,0);//850,1372 |
aoikoizumi | 4:e3d739bf09b4 | 314 | } |
aoikoizumi | 4:e3d739bf09b4 | 315 | if(i==5) { |
aoikoizumi | 5:0160b73f3d9e | 316 | susumu_y(1,1,1700);//850,1700 |
aoikoizumi | 4:e3d739bf09b4 | 317 | } |
aoikoizumi | 4:e3d739bf09b4 | 318 | if(i==6) { |
aoikoizumi | 4:e3d739bf09b4 | 319 | motorR.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 320 | motorL.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 321 | t.start(); |
aoikoizumi | 4:e3d739bf09b4 | 322 | if(t<1) { |
aoikoizumi | 4:e3d739bf09b4 | 323 | motor_f=0.82; |
aoikoizumi | 4:e3d739bf09b4 | 324 | motor_b=0; |
aoikoizumi | 4:e3d739bf09b4 | 325 | } else { |
aoikoizumi | 4:e3d739bf09b4 | 326 | motor_f=0; |
aoikoizumi | 4:e3d739bf09b4 | 327 | motor_b=0; |
aoikoizumi | 4:e3d739bf09b4 | 328 | printf("finish"); |
aoikoizumi | 4:e3d739bf09b4 | 329 | t.reset(); |
aoikoizumi | 4:e3d739bf09b4 | 330 | i++; |
aoikoizumi | 3:7bd1afb46094 | 331 | } |
aoikoizumi | 4:e3d739bf09b4 | 332 | }//gatiasari |
aoikoizumi | 3:7bd1afb46094 | 333 | |
aoikoizumi | 3:7bd1afb46094 | 334 | |
aoikoizumi | 4:e3d739bf09b4 | 335 | if(i==7) { |
aoikoizumi | 4:e3d739bf09b4 | 336 | t.start(); |
aoikoizumi | 4:e3d739bf09b4 | 337 | if(t<1) { |
aoikoizumi | 4:e3d739bf09b4 | 338 | motor_f=0; |
aoikoizumi | 4:e3d739bf09b4 | 339 | motor_b=0.82; |
aoikoizumi | 4:e3d739bf09b4 | 340 | } else { |
aoikoizumi | 4:e3d739bf09b4 | 341 | motor_f=0; |
aoikoizumi | 4:e3d739bf09b4 | 342 | motor_b=0; |
aoikoizumi | 4:e3d739bf09b4 | 343 | printf("finish"); |
aoikoizumi | 4:e3d739bf09b4 | 344 | t.reset(); |
aoikoizumi | 3:7bd1afb46094 | 345 | } |
aoikoizumi | 4:e3d739bf09b4 | 346 | if(angle>=-89) { |
aoikoizumi | 4:e3d739bf09b4 | 347 | target_R=BASIC_SPEED/5; |
aoikoizumi | 4:e3d739bf09b4 | 348 | target_L=BASIC_SPEED/(-5); |
aoikoizumi | 3:7bd1afb46094 | 349 | } |
aoikoizumi | 4:e3d739bf09b4 | 350 | if(angle<=-91) { |
aoikoizumi | 4:e3d739bf09b4 | 351 | target_R=BASIC_SPEED/(-5); |
aoikoizumi | 4:e3d739bf09b4 | 352 | target_L=BASIC_SPEED/-5; |
aoikoizumi | 3:7bd1afb46094 | 353 | } |
aoikoizumi | 4:e3d739bf09b4 | 354 | if(angle>-91&angle<-89) { |
aoikoizumi | 3:7bd1afb46094 | 355 | motorR.stop(); |
aoikoizumi | 3:7bd1afb46094 | 356 | motorL.stop(); |
aoikoizumi | 3:7bd1afb46094 | 357 | wait(0.5); |
aoikoizumi | 4:e3d739bf09b4 | 358 | if(angle>-91&angle<-89) { |
aoikoizumi | 4:e3d739bf09b4 | 359 | i++; |
aoikoizumi | 4:e3d739bf09b4 | 360 | } |
aoikoizumi | 3:7bd1afb46094 | 361 | } |
aoikoizumi | 4:e3d739bf09b4 | 362 | }//kakudo tyousei |
aoikoizumi | 4:e3d739bf09b4 | 363 | if(i==8) { |
aoikoizumi | 5:0160b73f3d9e | 364 | susumu_xr(1,1,700);//700,1700 |
aoikoizumi | 4:e3d739bf09b4 | 365 | } |
aoikoizumi | 4:e3d739bf09b4 | 366 | if(i==9) { |
aoikoizumi | 5:0160b73f3d9e | 367 | susumu_ang(1/3,1,0);//390,2010 |
aoikoizumi | 4:e3d739bf09b4 | 368 | } |
aoikoizumi | 4:e3d739bf09b4 | 369 | if(i==10) { |
aoikoizumi | 5:0160b73f3d9e | 370 | susumu_y(1,1,asari_y-310);//390,y-310 |
aoikoizumi | 4:e3d739bf09b4 | 371 | } |
aoikoizumi | 4:e3d739bf09b4 | 372 | if(i==11) { |
aoikoizumi | 5:0160b73f3d9e | 373 | susumu_ang(1/3,1,90);//700,y |
aoikoizumi | 4:e3d739bf09b4 | 374 | } |
aoikoizumi | 4:e3d739bf09b4 | 375 | if(i==12) { |
aoikoizumi | 4:e3d739bf09b4 | 376 | motorR.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 377 | motorL.stop(); |
aoikoizumi | 6:7ec6c3d3a30a | 378 | servo.pulsewidth_us(2100); |
aoikoizumi | 6:7ec6c3d3a30a | 379 | wait(1.5); |
aoikoizumi | 6:7ec6c3d3a30a | 380 | servo.pulsewidth_us(2400); |
aoikoizumi | 5:0160b73f3d9e | 381 | wait(1); |
aoikoizumi | 4:e3d739bf09b4 | 382 | i++; |
aoikoizumi | 4:e3d739bf09b4 | 383 | } |
aoikoizumi | 4:e3d739bf09b4 | 384 | if(i==13) { |
aoikoizumi | 4:e3d739bf09b4 | 385 | if(angle>=91) { |
aoikoizumi | 4:e3d739bf09b4 | 386 | target_R=BASIC_SPEED/5; |
aoikoizumi | 4:e3d739bf09b4 | 387 | target_L=BASIC_SPEED/(-5); |
aoikoizumi | 3:7bd1afb46094 | 388 | } |
aoikoizumi | 4:e3d739bf09b4 | 389 | if(angle<=89) { |
aoikoizumi | 4:e3d739bf09b4 | 390 | target_R=BASIC_SPEED/(-5); |
aoikoizumi | 4:e3d739bf09b4 | 391 | target_L=BASIC_SPEED/5; |
aoikoizumi | 3:7bd1afb46094 | 392 | } |
aoikoizumi | 4:e3d739bf09b4 | 393 | if(angle>89&angle<91) { |
aoikoizumi | 3:7bd1afb46094 | 394 | motorR.stop(); |
aoikoizumi | 3:7bd1afb46094 | 395 | motorL.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 396 | wait(0.1); |
aoikoizumi | 4:e3d739bf09b4 | 397 | if(angle>89&angle<91) { |
aoikoizumi | 4:e3d739bf09b4 | 398 | i++; |
aoikoizumi | 4:e3d739bf09b4 | 399 | } |
aoikoizumi | 4:e3d739bf09b4 | 400 | } |
aoikoizumi | 4:e3d739bf09b4 | 401 | } |
aoikoizumi | 4:e3d739bf09b4 | 402 | if(i==14) { |
aoikoizumi | 5:0160b73f3d9e | 403 | susumu_xl(1,1,asari_x);//x,y |
aoikoizumi | 4:e3d739bf09b4 | 404 | } |
aoikoizumi | 4:e3d739bf09b4 | 405 | if(i==15) { |
aoikoizumi | 4:e3d739bf09b4 | 406 | motorR.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 407 | motorL.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 408 | wait(0.5); |
aoikoizumi | 4:e3d739bf09b4 | 409 | denjiben=1; |
aoikoizumi | 4:e3d739bf09b4 | 410 | wait(0.5); |
aoikoizumi | 5:0160b73f3d9e | 411 | servo.pulsewidth_us(900); |
aoikoizumi | 6:7ec6c3d3a30a | 412 | wait(2); |
aoikoizumi | 4:e3d739bf09b4 | 413 | i++; |
aoikoizumi | 4:e3d739bf09b4 | 414 | } |
aoikoizumi | 4:e3d739bf09b4 | 415 | if(i==16) { |
aoikoizumi | 5:0160b73f3d9e | 416 | susumu_xr(-1,-1,700);//700,y |
aoikoizumi | 4:e3d739bf09b4 | 417 | } |
aoikoizumi | 4:e3d739bf09b4 | 418 | if(i==17) { |
aoikoizumi | 5:0160b73f3d9e | 419 | susumu_ang(-1/3,-1,0);//390,y-310 |
aoikoizumi | 4:e3d739bf09b4 | 420 | } |
aoikoizumi | 4:e3d739bf09b4 | 421 | if(i==18) { |
aoikoizumi | 5:0160b73f3d9e | 422 | susumu_y(-1,-1,goal_y+310);//390,y+310 |
aoikoizumi | 4:e3d739bf09b4 | 423 | } |
aoikoizumi | 4:e3d739bf09b4 | 424 | if(i==19) { |
aoikoizumi | 5:0160b73f3d9e | 425 | susumu_ang(-1/3,-1,-90);//700,y |
aoikoizumi | 4:e3d739bf09b4 | 426 | } |
aoikoizumi | 4:e3d739bf09b4 | 427 | if(i==20) { |
aoikoizumi | 5:0160b73f3d9e | 428 | susumu_xl(-1,-1,goal_x);//x,y |
aoikoizumi | 4:e3d739bf09b4 | 429 | } |
aoikoizumi | 4:e3d739bf09b4 | 430 | if(i==21) { |
aoikoizumi | 4:e3d739bf09b4 | 431 | motorR.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 432 | motorL.stop(); |
aoikoizumi | 4:e3d739bf09b4 | 433 | wait(0.5); |
aoikoizumi | 4:e3d739bf09b4 | 434 | denjiben=0; |
aoikoizumi | 4:e3d739bf09b4 | 435 | wait(0.5); |
aoikoizumi | 0:29c024d6882f | 436 | |
aoikoizumi | 4:e3d739bf09b4 | 437 | break; |
aoikoizumi | 4:e3d739bf09b4 | 438 | } |
aoikoizumi | 0:29c024d6882f | 439 | |
aoikoizumi | 0:29c024d6882f | 440 | |
aoikoizumi | 4:e3d739bf09b4 | 441 | }//while |
aoikoizumi | 4:e3d739bf09b4 | 442 | tgt(0,0); |
aoikoizumi | 4:e3d739bf09b4 | 443 | return 0; |
aoikoizumi | 3:7bd1afb46094 | 444 | |
aoikoizumi | 0:29c024d6882f | 445 | }//intmain |
aoikoizumi | 0:29c024d6882f | 446 |