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