春ロボ ロケット団
/
Right_F3RC_mar4_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Encoder CruizCore_R1370P
main.cpp@3:8b9388a6cabe, 2019-10-07 (annotated)
- Committer:
- koheim
- Date:
- Mon Oct 07 13:59:50 2019 +0000
- Revision:
- 3:8b9388a6cabe
- Parent:
- 2:8ab2c4ec07e7
aaa
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| maxnagazumi | 0:9c9994747ea7 | 1 | #include "mbed.h" |
| maxnagazumi | 0:9c9994747ea7 | 2 | #include "math.h" |
| maxnagazumi | 0:9c9994747ea7 | 3 | #include "R1370P.h" |
| maxnagazumi | 0:9c9994747ea7 | 4 | #include "EC.h" |
| maxnagazumi | 0:9c9994747ea7 | 5 | #define RESOLUTION 500 |
| maxnagazumi | 0:9c9994747ea7 | 6 | PwmOut TB(PB_8);//Top motor Forward |
| maxnagazumi | 0:9c9994747ea7 | 7 | PwmOut TF(PC_9);//Top motor Backward |
| maxnagazumi | 0:9c9994747ea7 | 8 | PwmOut RF(PC_8);//Right motor is okay |
| maxnagazumi | 0:9c9994747ea7 | 9 | PwmOut RB(PC_6); |
| maxnagazumi | 0:9c9994747ea7 | 10 | PwmOut LF(PB_13);//Left motor ONLY F is okay |
| maxnagazumi | 0:9c9994747ea7 | 11 | PwmOut LB(PB_15); |
| maxnagazumi | 0:9c9994747ea7 | 12 | |
| maxnagazumi | 0:9c9994747ea7 | 13 | PwmOut servo(PA_15); |
| maxnagazumi | 0:9c9994747ea7 | 14 | |
| maxnagazumi | 0:9c9994747ea7 | 15 | DigitalOut led1(LED1); |
| maxnagazumi | 0:9c9994747ea7 | 16 | DigitalIn button(USER_BUTTON); |
| maxnagazumi | 0:9c9994747ea7 | 17 | DigitalOut out1(PB_9); |
| maxnagazumi | 0:9c9994747ea7 | 18 | DigitalIn RandL(PC_12); |
| maxnagazumi | 0:9c9994747ea7 | 19 | |
| maxnagazumi | 0:9c9994747ea7 | 20 | Serial pc(USBTX, USBRX); // tx, rx |
| maxnagazumi | 0:9c9994747ea7 | 21 | R1370P gyro(PA_11,PA_12); // tx, rx |
| maxnagazumi | 0:9c9994747ea7 | 22 | Ticker ticker; |
| maxnagazumi | 0:9c9994747ea7 | 23 | |
| maxnagazumi | 0:9c9994747ea7 | 24 | //Serial pc(USBTX,USBRX); |
| maxnagazumi | 0:9c9994747ea7 | 25 | Ec1multi EC(PA_8,PA_9,RESOLUTION); //1逓倍用class |
| maxnagazumi | 0:9c9994747ea7 | 26 | Ec1multi EC2(PB_3,PB_5,RESOLUTION); |
| maxnagazumi | 0:9c9994747ea7 | 27 | |
| maxnagazumi | 0:9c9994747ea7 | 28 | void rotation(double angle) |
| maxnagazumi | 0:9c9994747ea7 | 29 | { |
| maxnagazumi | 0:9c9994747ea7 | 30 | double a,b; |
| maxnagazumi | 0:9c9994747ea7 | 31 | if( angle > 1) { |
| maxnagazumi | 1:1cb20fa989c2 | 32 | a = 0.05; |
| maxnagazumi | 0:9c9994747ea7 | 33 | b = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 34 | RF = RF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 35 | RB = RB + b; |
| maxnagazumi | 1:1cb20fa989c2 | 36 | LF = LF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 37 | LB = LB + b; |
| maxnagazumi | 1:1cb20fa989c2 | 38 | TF = TF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 39 | TB = TB + b; |
| maxnagazumi | 0:9c9994747ea7 | 40 | } else if(angle < -1) { |
| maxnagazumi | 0:9c9994747ea7 | 41 | a = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 42 | b = 0.05; |
| maxnagazumi | 1:1cb20fa989c2 | 43 | RF = RF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 44 | RB = RB + b; |
| maxnagazumi | 1:1cb20fa989c2 | 45 | LF = LF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 46 | LB = LB + b; |
| maxnagazumi | 1:1cb20fa989c2 | 47 | TF = TF + a; |
| maxnagazumi | 1:1cb20fa989c2 | 48 | TB = TB + b; |
| maxnagazumi | 0:9c9994747ea7 | 49 | } |
| maxnagazumi | 0:9c9994747ea7 | 50 | |
| maxnagazumi | 0:9c9994747ea7 | 51 | } |
| maxnagazumi | 0:9c9994747ea7 | 52 | |
| maxnagazumi | 1:1cb20fa989c2 | 53 | void go_S(double a,int x,double b,int count) |
| maxnagazumi | 2:8ab2c4ec07e7 | 54 | //b is value for going right or left |
| maxnagazumi | 2:8ab2c4ec07e7 | 55 | //count is value for staying right potition of x-axis |
| maxnagazumi | 0:9c9994747ea7 | 56 | { |
| koheim | 3:8b9388a6cabe | 57 | if((x - count)*(x - count) < 144) {// |
| maxnagazumi | 1:1cb20fa989c2 | 58 | RF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 59 | RB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 60 | LF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 61 | LB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 62 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 63 | TB = 0; |
| koheim | 3:8b9388a6cabe | 64 | } else if(x - count >= 12) {// |
| maxnagazumi | 1:1cb20fa989c2 | 65 | RF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 66 | RB = a + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 67 | LF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 68 | LB = 0 + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 69 | TF = 0 + b; |
| maxnagazumi | 1:1cb20fa989c2 | 70 | TB = 0; |
| koheim | 3:8b9388a6cabe | 71 | } else if (x - count <= -12) {// |
| maxnagazumi | 1:1cb20fa989c2 | 72 | RF = 0 + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 73 | RB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 74 | LF = a + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 75 | LB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 76 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 77 | TB = 0 + b; |
| maxnagazumi | 1:1cb20fa989c2 | 78 | } |
| maxnagazumi | 0:9c9994747ea7 | 79 | } |
| maxnagazumi | 0:9c9994747ea7 | 80 | |
| maxnagazumi | 1:1cb20fa989c2 | 81 | void go_B(double a,int x,double b,int count) |
| maxnagazumi | 2:8ab2c4ec07e7 | 82 | // b and count are same as go_S |
| maxnagazumi | 0:9c9994747ea7 | 83 | { |
| koheim | 3:8b9388a6cabe | 84 | if((x - count)*(x - count) < 144) {// |
| maxnagazumi | 1:1cb20fa989c2 | 85 | RF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 86 | RB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 87 | LF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 88 | LB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 89 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 90 | TB = 0; |
| koheim | 3:8b9388a6cabe | 91 | } else if(x - count >= 12) {// |
| maxnagazumi | 1:1cb20fa989c2 | 92 | RF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 93 | RB = 0 + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 94 | LF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 95 | LB = a + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 96 | TF = 0 + b; |
| maxnagazumi | 1:1cb20fa989c2 | 97 | TB = 0; |
| koheim | 3:8b9388a6cabe | 98 | } else if (x - count <= -12) {// |
| maxnagazumi | 1:1cb20fa989c2 | 99 | RF = a + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 100 | RB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 101 | LF = 0 + b/2; |
| maxnagazumi | 1:1cb20fa989c2 | 102 | LB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 103 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 104 | TB = 0 + b; |
| maxnagazumi | 1:1cb20fa989c2 | 105 | } |
| maxnagazumi | 0:9c9994747ea7 | 106 | } |
| maxnagazumi | 0:9c9994747ea7 | 107 | |
| maxnagazumi | 0:9c9994747ea7 | 108 | void go_R(double a) |
| maxnagazumi | 0:9c9994747ea7 | 109 | { |
| maxnagazumi | 0:9c9994747ea7 | 110 | TB = a; |
| maxnagazumi | 0:9c9994747ea7 | 111 | TF = 0; |
| maxnagazumi | 0:9c9994747ea7 | 112 | RF = a/2; |
| maxnagazumi | 0:9c9994747ea7 | 113 | RB = 0; |
| koheim | 3:8b9388a6cabe | 114 | LF = a/4;// |
| maxnagazumi | 0:9c9994747ea7 | 115 | LB = 0; |
| maxnagazumi | 0:9c9994747ea7 | 116 | } |
| maxnagazumi | 0:9c9994747ea7 | 117 | |
| maxnagazumi | 0:9c9994747ea7 | 118 | void go_L(double a) |
| maxnagazumi | 0:9c9994747ea7 | 119 | { |
| maxnagazumi | 0:9c9994747ea7 | 120 | TB = 0; |
| maxnagazumi | 0:9c9994747ea7 | 121 | TF = a; |
| maxnagazumi | 0:9c9994747ea7 | 122 | RF = 0; |
| maxnagazumi | 0:9c9994747ea7 | 123 | RB = a/2; |
| maxnagazumi | 0:9c9994747ea7 | 124 | LF = 0; |
| maxnagazumi | 0:9c9994747ea7 | 125 | LB = a/2; |
| maxnagazumi | 0:9c9994747ea7 | 126 | } |
| maxnagazumi | 0:9c9994747ea7 | 127 | |
| maxnagazumi | 0:9c9994747ea7 | 128 | int main() |
| maxnagazumi | 0:9c9994747ea7 | 129 | { |
| maxnagazumi | 1:1cb20fa989c2 | 130 | double a,b; |
| maxnagazumi | 0:9c9994747ea7 | 131 | TF.period_ms(1); |
| maxnagazumi | 0:9c9994747ea7 | 132 | TB.period_ms(1); |
| maxnagazumi | 0:9c9994747ea7 | 133 | RF.period_ms(1); |
| maxnagazumi | 0:9c9994747ea7 | 134 | RB.period_ms(1); |
| maxnagazumi | 0:9c9994747ea7 | 135 | LF.period_ms(1); |
| maxnagazumi | 0:9c9994747ea7 | 136 | LB.period_ms(1); |
| maxnagazumi | 1:1cb20fa989c2 | 137 | out1 = 0; |
| maxnagazumi | 0:9c9994747ea7 | 138 | servo.period_ms(20); |
| maxnagazumi | 0:9c9994747ea7 | 139 | servo.pulsewidth_us(550); |
| maxnagazumi | 0:9c9994747ea7 | 140 | wait(1); |
| maxnagazumi | 0:9c9994747ea7 | 141 | int count=0,count2=0; |
| maxnagazumi | 0:9c9994747ea7 | 142 | double angle; |
| maxnagazumi | 0:9c9994747ea7 | 143 | //ticker.attach(&calcOmega,0.5); |
| maxnagazumi | 0:9c9994747ea7 | 144 | gyro.initialize(); //main関数の最初に一度だけ実行 |
| maxnagazumi | 0:9c9994747ea7 | 145 | gyro.acc_offset(); //やってもやらなくてもいい |
| maxnagazumi | 0:9c9994747ea7 | 146 | |
| maxnagazumi | 0:9c9994747ea7 | 147 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 148 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 149 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 150 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 151 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 152 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 153 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 154 | |
| maxnagazumi | 0:9c9994747ea7 | 155 | while(count < 200) { |
| maxnagazumi | 0:9c9994747ea7 | 156 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 157 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 158 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 159 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 160 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 161 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 162 | pc.printf("first"); |
| maxnagazumi | 0:9c9994747ea7 | 163 | pc.printf("\r\n"); |
| maxnagazumi | 1:1cb20fa989c2 | 164 | go_R(0.2); |
| maxnagazumi | 0:9c9994747ea7 | 165 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 166 | } |
| maxnagazumi | 1:1cb20fa989c2 | 167 | go_R(0); |
| maxnagazumi | 0:9c9994747ea7 | 168 | rotation(angle); |
| maxnagazumi | 1:1cb20fa989c2 | 169 | while(count2 < 10000) { |
| maxnagazumi | 1:1cb20fa989c2 | 170 | count=EC.getCount(); |
| maxnagazumi | 1:1cb20fa989c2 | 171 | count2=EC2.getCount(); |
| maxnagazumi | 1:1cb20fa989c2 | 172 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 1:1cb20fa989c2 | 173 | pc.printf(" x=%d ",count); |
| maxnagazumi | 1:1cb20fa989c2 | 174 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 1:1cb20fa989c2 | 175 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 1:1cb20fa989c2 | 176 | pc.printf("\r\n"); |
| koheim | 3:8b9388a6cabe | 177 | go_S(/*0.4*/0.5,200,0.1,count);// |
| maxnagazumi | 1:1cb20fa989c2 | 178 | rotation(angle); |
| maxnagazumi | 1:1cb20fa989c2 | 179 | } |
| maxnagazumi | 1:1cb20fa989c2 | 180 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 181 | while(count2 < 10800) { |
| maxnagazumi | 0:9c9994747ea7 | 182 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 183 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 184 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 185 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 186 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 187 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 188 | pc.printf("\r\n"); |
| koheim | 3:8b9388a6cabe | 189 | go_S(/*0.2*/0.3,200,0.1,count);// |
| maxnagazumi | 0:9c9994747ea7 | 190 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 191 | } |
| maxnagazumi | 1:1cb20fa989c2 | 192 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 193 | while(count < 195) { |
| maxnagazumi | 0:9c9994747ea7 | 194 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 195 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 196 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 197 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 198 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 199 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 200 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 201 | go_R(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 202 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 203 | } |
| maxnagazumi | 0:9c9994747ea7 | 204 | go_R(0); |
| maxnagazumi | 0:9c9994747ea7 | 205 | while(count > 205) { |
| maxnagazumi | 0:9c9994747ea7 | 206 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 207 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 208 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 209 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 210 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 211 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 212 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 213 | go_L(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 214 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 215 | } |
| maxnagazumi | 0:9c9994747ea7 | 216 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 217 | servo.pulsewidth_us(600); |
| maxnagazumi | 0:9c9994747ea7 | 218 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 219 | servo.pulsewidth_us(700); |
| maxnagazumi | 0:9c9994747ea7 | 220 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 221 | servo.pulsewidth_us(750); |
| maxnagazumi | 0:9c9994747ea7 | 222 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 223 | servo.pulsewidth_us(800); |
| maxnagazumi | 0:9c9994747ea7 | 224 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 225 | servo.pulsewidth_us(850); |
| maxnagazumi | 0:9c9994747ea7 | 226 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 227 | servo.pulsewidth_us(900); |
| maxnagazumi | 0:9c9994747ea7 | 228 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 229 | servo.pulsewidth_us(950); |
| maxnagazumi | 0:9c9994747ea7 | 230 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 231 | servo.pulsewidth_us(1000); |
| maxnagazumi | 0:9c9994747ea7 | 232 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 233 | servo.pulsewidth_us(1050); |
| maxnagazumi | 0:9c9994747ea7 | 234 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 235 | servo.pulsewidth_us(1070); |
| maxnagazumi | 0:9c9994747ea7 | 236 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 237 | servo.pulsewidth_us(1100); |
| maxnagazumi | 0:9c9994747ea7 | 238 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 239 | servo.pulsewidth_us(1180); |
| maxnagazumi | 0:9c9994747ea7 | 240 | wait(1); |
| maxnagazumi | 0:9c9994747ea7 | 241 | out1 = 1;//catch |
| maxnagazumi | 0:9c9994747ea7 | 242 | wait(2); |
| maxnagazumi | 0:9c9994747ea7 | 243 | servo.pulsewidth_us(1150); |
| maxnagazumi | 0:9c9994747ea7 | 244 | //have caught |
| maxnagazumi | 0:9c9994747ea7 | 245 | while(count2 > 200) { |
| maxnagazumi | 0:9c9994747ea7 | 246 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 247 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 248 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 249 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 250 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 251 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 252 | pc.printf("\r\n"); |
| koheim | 3:8b9388a6cabe | 253 | go_B(/*0.4*/0.5,200,0.1,count);// |
| maxnagazumi | 0:9c9994747ea7 | 254 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 255 | } |
| maxnagazumi | 1:1cb20fa989c2 | 256 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 257 | while(count < 195) { |
| maxnagazumi | 0:9c9994747ea7 | 258 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 259 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 260 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 261 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 262 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 263 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 264 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 265 | go_R(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 266 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 267 | } |
| maxnagazumi | 1:1cb20fa989c2 | 268 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 269 | while(count > 205) { |
| maxnagazumi | 0:9c9994747ea7 | 270 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 271 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 272 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 273 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 274 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 275 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 276 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 277 | go_L(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 278 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 279 | } |
| maxnagazumi | 1:1cb20fa989c2 | 280 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 281 | while(angle > -85) { |
| maxnagazumi | 0:9c9994747ea7 | 282 | a = 0.05; |
| maxnagazumi | 0:9c9994747ea7 | 283 | b = 0; |
| maxnagazumi | 0:9c9994747ea7 | 284 | RF = a; |
| maxnagazumi | 0:9c9994747ea7 | 285 | RB = b; |
| maxnagazumi | 0:9c9994747ea7 | 286 | LF = a; |
| maxnagazumi | 0:9c9994747ea7 | 287 | LB = b; |
| maxnagazumi | 0:9c9994747ea7 | 288 | TF = a; |
| maxnagazumi | 0:9c9994747ea7 | 289 | TB = b; |
| maxnagazumi | 0:9c9994747ea7 | 290 | angle=gyro.getAngle(); |
| maxnagazumi | 0:9c9994747ea7 | 291 | } |
| maxnagazumi | 1:1cb20fa989c2 | 292 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 293 | pc.printf("1\r\n"); |
| maxnagazumi | 2:8ab2c4ec07e7 | 294 | //now I dont have any idea for this part. |
| maxnagazumi | 2:8ab2c4ec07e7 | 295 | //We should make a code to keep x(count) and y(count2) using math.h like cos() or sin() |
| maxnagazumi | 0:9c9994747ea7 | 296 | while(count2 < 1200) { |
| maxnagazumi | 0:9c9994747ea7 | 297 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 298 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 299 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 300 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 301 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 302 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 303 | pc.printf("\r\n"); |
| maxnagazumi | 1:1cb20fa989c2 | 304 | a = 0.1; |
| maxnagazumi | 1:1cb20fa989c2 | 305 | RF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 306 | RB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 307 | LF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 308 | LB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 309 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 310 | TB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 311 | } |
| maxnagazumi | 0:9c9994747ea7 | 312 | pc.printf("2\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 313 | go_L(0); |
| maxnagazumi | 2:8ab2c4ec07e7 | 314 | wait(10); //get object |
| maxnagazumi | 0:9c9994747ea7 | 315 | pc.printf("3\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 316 | while(count2 > 500) { |
| maxnagazumi | 0:9c9994747ea7 | 317 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 318 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 319 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 320 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 321 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 322 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 323 | pc.printf("\r\n"); |
| maxnagazumi | 1:1cb20fa989c2 | 324 | a = 0.1; |
| maxnagazumi | 1:1cb20fa989c2 | 325 | RF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 326 | RB = a; |
| maxnagazumi | 1:1cb20fa989c2 | 327 | LF = a; |
| maxnagazumi | 1:1cb20fa989c2 | 328 | LB = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 329 | TF = 0; |
| maxnagazumi | 1:1cb20fa989c2 | 330 | TB = 0; |
| maxnagazumi | 0:9c9994747ea7 | 331 | } |
| maxnagazumi | 1:1cb20fa989c2 | 332 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 333 | pc.printf("4\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 334 | while( angle > 1) { |
| maxnagazumi | 0:9c9994747ea7 | 335 | double a,b; |
| maxnagazumi | 0:9c9994747ea7 | 336 | angle=gyro.getAngle(); |
| maxnagazumi | 0:9c9994747ea7 | 337 | a = 0.05; |
| maxnagazumi | 0:9c9994747ea7 | 338 | b = 0; |
| maxnagazumi | 0:9c9994747ea7 | 339 | RF = a; |
| maxnagazumi | 0:9c9994747ea7 | 340 | RB = b; |
| maxnagazumi | 0:9c9994747ea7 | 341 | LF = a; |
| maxnagazumi | 0:9c9994747ea7 | 342 | LB = b; |
| maxnagazumi | 0:9c9994747ea7 | 343 | TF = a; |
| maxnagazumi | 0:9c9994747ea7 | 344 | TB = b; |
| maxnagazumi | 0:9c9994747ea7 | 345 | } |
| maxnagazumi | 1:1cb20fa989c2 | 346 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 347 | while(angle < -1) { |
| maxnagazumi | 0:9c9994747ea7 | 348 | double a,b; |
| maxnagazumi | 0:9c9994747ea7 | 349 | angle=gyro.getAngle(); |
| maxnagazumi | 0:9c9994747ea7 | 350 | a = 0; |
| maxnagazumi | 0:9c9994747ea7 | 351 | b = 0.05; |
| maxnagazumi | 0:9c9994747ea7 | 352 | RF = a; |
| maxnagazumi | 0:9c9994747ea7 | 353 | RB = b; |
| maxnagazumi | 0:9c9994747ea7 | 354 | LF = a; |
| maxnagazumi | 0:9c9994747ea7 | 355 | LB = b; |
| maxnagazumi | 0:9c9994747ea7 | 356 | TF = a; |
| maxnagazumi | 0:9c9994747ea7 | 357 | TB = b; |
| maxnagazumi | 0:9c9994747ea7 | 358 | } |
| maxnagazumi | 1:1cb20fa989c2 | 359 | go_L(0); |
| maxnagazumi | 0:9c9994747ea7 | 360 | pc.printf("5\r\n"); |
| koheim | 3:8b9388a6cabe | 361 | int old_count; |
| koheim | 3:8b9388a6cabe | 362 | int old_count2; |
| maxnagazumi | 1:1cb20fa989c2 | 363 | old_count=EC.getCount(); |
| maxnagazumi | 1:1cb20fa989c2 | 364 | old_count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 365 | while(count2 < 11800) { |
| maxnagazumi | 0:9c9994747ea7 | 366 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 367 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 368 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 369 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 370 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 371 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 372 | pc.printf("\r\n"); |
| maxnagazumi | 1:1cb20fa989c2 | 373 | go_S(0.4,old_count,0.05,count); |
| maxnagazumi | 0:9c9994747ea7 | 374 | rotation(angle); |
| maxnagazumi | 1:1cb20fa989c2 | 375 | } |
| maxnagazumi | 0:9c9994747ea7 | 376 | while(count < 2500) { |
| maxnagazumi | 0:9c9994747ea7 | 377 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 378 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 379 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 380 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 381 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 382 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 383 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 384 | go_R(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 385 | rotation(angle); |
| maxnagazumi | 1:1cb20fa989c2 | 386 | } |
| maxnagazumi | 1:1cb20fa989c2 | 387 | old_count=EC.getCount(); |
| maxnagazumi | 1:1cb20fa989c2 | 388 | old_count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 389 | while(count2 < 13000) { |
| maxnagazumi | 0:9c9994747ea7 | 390 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 391 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 392 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 393 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 394 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 395 | pc.printf(" angle=%8.4f ",angle); |
| maxnagazumi | 0:9c9994747ea7 | 396 | pc.printf("\r\n"); |
| maxnagazumi | 1:1cb20fa989c2 | 397 | go_S(0.4,count,0.05,count); |
| maxnagazumi | 0:9c9994747ea7 | 398 | rotation(angle); |
| maxnagazumi | 1:1cb20fa989c2 | 399 | } |
| maxnagazumi | 1:1cb20fa989c2 | 400 | while(count > 600) { |
| maxnagazumi | 0:9c9994747ea7 | 401 | count=EC.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 402 | count2=EC2.getCount(); |
| maxnagazumi | 0:9c9994747ea7 | 403 | angle=gyro.getAngle(); //角度の値を受け取る |
| maxnagazumi | 0:9c9994747ea7 | 404 | pc.printf(" x=%d ",count); |
| maxnagazumi | 0:9c9994747ea7 | 405 | pc.printf(" y=%d ",count2); |
| maxnagazumi | 0:9c9994747ea7 | 406 | pc.printf(" angle=%8.4f",angle); |
| maxnagazumi | 0:9c9994747ea7 | 407 | pc.printf("\r\n"); |
| maxnagazumi | 0:9c9994747ea7 | 408 | go_L(0.1); |
| maxnagazumi | 0:9c9994747ea7 | 409 | rotation(angle); |
| maxnagazumi | 0:9c9994747ea7 | 410 | } |
| maxnagazumi | 0:9c9994747ea7 | 411 | go_R(0); |
| maxnagazumi | 0:9c9994747ea7 | 412 | servo.pulsewidth_us(1100); |
| maxnagazumi | 0:9c9994747ea7 | 413 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 414 | servo.pulsewidth_us(1050); |
| maxnagazumi | 0:9c9994747ea7 | 415 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 416 | servo.pulsewidth_us(1000); |
| maxnagazumi | 0:9c9994747ea7 | 417 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 418 | servo.pulsewidth_us(950); |
| maxnagazumi | 0:9c9994747ea7 | 419 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 420 | servo.pulsewidth_us(900); |
| maxnagazumi | 0:9c9994747ea7 | 421 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 422 | servo.pulsewidth_us(850); |
| maxnagazumi | 0:9c9994747ea7 | 423 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 424 | servo.pulsewidth_us(800); |
| maxnagazumi | 0:9c9994747ea7 | 425 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 426 | servo.pulsewidth_us(750); |
| maxnagazumi | 0:9c9994747ea7 | 427 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 428 | servo.pulsewidth_us(700); |
| maxnagazumi | 0:9c9994747ea7 | 429 | wait(0.5); |
| maxnagazumi | 0:9c9994747ea7 | 430 | servo.pulsewidth_us(650); |
| maxnagazumi | 0:9c9994747ea7 | 431 | wait(1); |
| maxnagazumi | 0:9c9994747ea7 | 432 | servo.pulsewidth_us(600); |
| maxnagazumi | 0:9c9994747ea7 | 433 | wait(1); |
| maxnagazumi | 0:9c9994747ea7 | 434 | servo.pulsewidth_us(550); |
| maxnagazumi | 0:9c9994747ea7 | 435 | wait(1); |
| maxnagazumi | 1:1cb20fa989c2 | 436 | } |