ロボステ6期
/
NHK2020-arm-sub4
12/20 mainmode3
main.cpp@2:90b53995cb1f, 2019-12-20 (annotated)
- Committer:
- aoikoizumi
- Date:
- Fri Dec 20 05:52:05 2019 +0000
- Revision:
- 2:90b53995cb1f
- Parent:
- 1:bc34fdc4e16b
- Child:
- 3:9cef6e58a462
12/20
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki0701 | 0:6e2abd0956f1 | 1 | #include "mbed.h" |
yuki0701 | 0:6e2abd0956f1 | 2 | #include "EC.h" //Encoderライブラリをインクルード |
yuki0701 | 0:6e2abd0956f1 | 3 | #include "SpeedController.h" |
yuki0701 | 0:6e2abd0956f1 | 4 | #define RESOLUTION 2048//分解能 |
yuki0701 | 0:6e2abd0956f1 | 5 | |
yuki0701 | 0:6e2abd0956f1 | 6 | //#define NHK2020_TEST_MODE |
yuki0701 | 0:6e2abd0956f1 | 7 | #define NHK2020_MAIN_MODE |
yuki0701 | 0:6e2abd0956f1 | 8 | |
yuki0701 | 0:6e2abd0956f1 | 9 | CAN can1(p30,p29); |
yuki0701 | 0:6e2abd0956f1 | 10 | |
yuki0701 | 0:6e2abd0956f1 | 11 | Ticker can_ticker; //can用ticker |
yuki0701 | 0:6e2abd0956f1 | 12 | |
yuki0701 | 0:6e2abd0956f1 | 13 | Ec4multi EC_backdrop(p15,p16,RESOLUTION); |
yuki0701 | 0:6e2abd0956f1 | 14 | SpeedControl backdrop(p21,p22,50,EC_backdrop); |
yuki0701 | 0:6e2abd0956f1 | 15 | Serial pc(USBTX,USBRX); |
yuki0701 | 0:6e2abd0956f1 | 16 | DigitalOut snatch(p8); |
yuki0701 | 0:6e2abd0956f1 | 17 | DigitalOut pass1(p27); |
yuki0701 | 0:6e2abd0956f1 | 18 | DigitalOut pass2(p28); |
yuki0701 | 0:6e2abd0956f1 | 19 | //Ticker motor_tick; //角速度計算用ticker |
yuki0701 | 0:6e2abd0956f1 | 20 | |
yuki0701 | 0:6e2abd0956f1 | 21 | char t2[1]= {0}; //動作番号(送信する値(char型)) |
yuki0701 | 0:6e2abd0956f1 | 22 | int t2_r=0, T2=0; //動作番号(受け取った値、送信する値(int型)) |
yuki0701 | 0:6e2abd0956f1 | 23 | |
yuki0701 | 0:6e2abd0956f1 | 24 | double a=0;//now |
yuki0701 | 0:6e2abd0956f1 | 25 | double b=0;//target |
yuki0701 | 0:6e2abd0956f1 | 26 | double turn=0; |
yuki0701 | 0:6e2abd0956f1 | 27 | int k = 0; |
yuki0701 | 0:6e2abd0956f1 | 28 | void tsukami() |
yuki0701 | 0:6e2abd0956f1 | 29 | { |
yuki0701 | 0:6e2abd0956f1 | 30 | b=1.3; |
yuki0701 | 0:6e2abd0956f1 | 31 | } |
yuki0701 | 0:6e2abd0956f1 | 32 | void put() |
yuki0701 | 0:6e2abd0956f1 | 33 | { |
yuki0701 | 0:6e2abd0956f1 | 34 | b=-0.7; |
yuki0701 | 0:6e2abd0956f1 | 35 | } |
yuki0701 | 0:6e2abd0956f1 | 36 | void top() |
yuki0701 | 0:6e2abd0956f1 | 37 | { |
yuki0701 | 0:6e2abd0956f1 | 38 | b=0; |
yuki0701 | 0:6e2abd0956f1 | 39 | } |
aoikoizumi | 1:bc34fdc4e16b | 40 | void move_motor() |
yuki0701 | 0:6e2abd0956f1 | 41 | { |
aoikoizumi | 1:bc34fdc4e16b | 42 | while(1) { |
aoikoizumi | 1:bc34fdc4e16b | 43 | double old_turn=turn; |
aoikoizumi | 1:bc34fdc4e16b | 44 | a=EC_backdrop.getRad(); |
aoikoizumi | 1:bc34fdc4e16b | 45 | if(a-b>=0.1) { |
aoikoizumi | 1:bc34fdc4e16b | 46 | turn=0.1; |
aoikoizumi | 1:bc34fdc4e16b | 47 | pc.printf("F"); |
aoikoizumi | 1:bc34fdc4e16b | 48 | } else if (a-b>=0.05) { |
aoikoizumi | 1:bc34fdc4e16b | 49 | turn=10*(a-b)*(a-b); |
aoikoizumi | 1:bc34fdc4e16b | 50 | pc.printf("f"); |
aoikoizumi | 1:bc34fdc4e16b | 51 | } else if (b-a>=0.1) { |
aoikoizumi | 1:bc34fdc4e16b | 52 | turn=-0.1; |
aoikoizumi | 1:bc34fdc4e16b | 53 | pc.printf("B"); |
aoikoizumi | 1:bc34fdc4e16b | 54 | } else if (b-a>=0.05) { |
aoikoizumi | 1:bc34fdc4e16b | 55 | turn=-10*(a-b)*(a-b); |
aoikoizumi | 1:bc34fdc4e16b | 56 | pc.printf("b"); |
aoikoizumi | 1:bc34fdc4e16b | 57 | } else { |
aoikoizumi | 1:bc34fdc4e16b | 58 | backdrop.stop(); |
aoikoizumi | 1:bc34fdc4e16b | 59 | backdrop.turn(0); |
aoikoizumi | 1:bc34fdc4e16b | 60 | turn=0; |
aoikoizumi | 1:bc34fdc4e16b | 61 | pc.printf("s"); |
aoikoizumi | 1:bc34fdc4e16b | 62 | } |
aoikoizumi | 1:bc34fdc4e16b | 63 | if(turn*old_turn<0)turn=0; |
aoikoizumi | 1:bc34fdc4e16b | 64 | backdrop.turn(turn); |
aoikoizumi | 1:bc34fdc4e16b | 65 | pc.printf("%lf",EC_backdrop.getRad()); |
aoikoizumi | 1:bc34fdc4e16b | 66 | if(b-a<0.05&&a-b<0.05)break; |
yuki0701 | 0:6e2abd0956f1 | 67 | } |
yuki0701 | 0:6e2abd0956f1 | 68 | } |
yuki0701 | 0:6e2abd0956f1 | 69 | |
yuki0701 | 0:6e2abd0956f1 | 70 | int q = 0; |
yuki0701 | 0:6e2abd0956f1 | 71 | |
yuki0701 | 0:6e2abd0956f1 | 72 | void can_read() |
yuki0701 | 0:6e2abd0956f1 | 73 | { |
yuki0701 | 0:6e2abd0956f1 | 74 | |
yuki0701 | 0:6e2abd0956f1 | 75 | |
yuki0701 | 0:6e2abd0956f1 | 76 | CANMessage msg; |
yuki0701 | 0:6e2abd0956f1 | 77 | |
yuki0701 | 0:6e2abd0956f1 | 78 | if(can1.read(msg)) { |
yuki0701 | 0:6e2abd0956f1 | 79 | |
yuki0701 | 0:6e2abd0956f1 | 80 | if(msg.id == 1) { |
yuki0701 | 0:6e2abd0956f1 | 81 | t2_r = msg.data[0]; |
yuki0701 | 0:6e2abd0956f1 | 82 | //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 83 | } else { |
yuki0701 | 0:6e2abd0956f1 | 84 | if(q > 100) { |
yuki0701 | 0:6e2abd0956f1 | 85 | //printf("id1 fale\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 86 | q = 0; |
yuki0701 | 0:6e2abd0956f1 | 87 | } |
yuki0701 | 0:6e2abd0956f1 | 88 | q++; |
yuki0701 | 0:6e2abd0956f1 | 89 | } |
yuki0701 | 0:6e2abd0956f1 | 90 | |
yuki0701 | 0:6e2abd0956f1 | 91 | } else { |
yuki0701 | 0:6e2abd0956f1 | 92 | if(k > 100) { |
yuki0701 | 0:6e2abd0956f1 | 93 | //printf("arm fale\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 94 | k = 0; |
yuki0701 | 0:6e2abd0956f1 | 95 | } |
yuki0701 | 0:6e2abd0956f1 | 96 | k++; |
yuki0701 | 0:6e2abd0956f1 | 97 | } |
yuki0701 | 0:6e2abd0956f1 | 98 | |
yuki0701 | 0:6e2abd0956f1 | 99 | t2[0] = T2; |
yuki0701 | 0:6e2abd0956f1 | 100 | |
yuki0701 | 0:6e2abd0956f1 | 101 | if(can1.write(CANMessage(2,t2,1))) { |
yuki0701 | 0:6e2abd0956f1 | 102 | } |
yuki0701 | 0:6e2abd0956f1 | 103 | |
yuki0701 | 0:6e2abd0956f1 | 104 | if(t2_r > T2) { |
yuki0701 | 0:6e2abd0956f1 | 105 | T2 = t2_r; |
yuki0701 | 0:6e2abd0956f1 | 106 | } |
yuki0701 | 0:6e2abd0956f1 | 107 | |
yuki0701 | 0:6e2abd0956f1 | 108 | |
yuki0701 | 0:6e2abd0956f1 | 109 | } |
yuki0701 | 0:6e2abd0956f1 | 110 | |
yuki0701 | 0:6e2abd0956f1 | 111 | |
yuki0701 | 0:6e2abd0956f1 | 112 | int main() |
yuki0701 | 0:6e2abd0956f1 | 113 | { |
yuki0701 | 0:6e2abd0956f1 | 114 | pc.printf("setting please"); |
aoikoizumi | 1:bc34fdc4e16b | 115 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 116 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 117 | |
yuki0701 | 0:6e2abd0956f1 | 118 | |
yuki0701 | 0:6e2abd0956f1 | 119 | |
yuki0701 | 0:6e2abd0956f1 | 120 | |
yuki0701 | 0:6e2abd0956f1 | 121 | #ifdef NHK2020_TEST_MODE |
aoikoizumi | 2:90b53995cb1f | 122 | printf("T2 = %d\n\r",T2); |
yuki0701 | 0:6e2abd0956f1 | 123 | pc.printf("%lf",EC_backdrop.getRad()); |
yuki0701 | 0:6e2abd0956f1 | 124 | if(pc.readable()) { |
yuki0701 | 0:6e2abd0956f1 | 125 | char sel=pc.getc(); |
yuki0701 | 0:6e2abd0956f1 | 126 | if(sel=='z') { |
yuki0701 | 0:6e2abd0956f1 | 127 | pc.printf("z\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 128 | tsukami(); |
yuki0701 | 0:6e2abd0956f1 | 129 | } else if(sel=='x') { |
yuki0701 | 0:6e2abd0956f1 | 130 | pc.printf("x\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 131 | put(); |
yuki0701 | 0:6e2abd0956f1 | 132 | } else if(sel=='c') { |
yuki0701 | 0:6e2abd0956f1 | 133 | pc.printf("c\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 134 | top(); |
yuki0701 | 0:6e2abd0956f1 | 135 | } |
yuki0701 | 0:6e2abd0956f1 | 136 | /* if(sel=='q') { |
yuki0701 | 0:6e2abd0956f1 | 137 | printf("\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 138 | if(denjiben==0)denjiben=1; |
yuki0701 | 0:6e2abd0956f1 | 139 | else denjiben=0; |
yuki0701 | 0:6e2abd0956f1 | 140 | }*/ |
yuki0701 | 0:6e2abd0956f1 | 141 | if(sel=='1') { |
yuki0701 | 0:6e2abd0956f1 | 142 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 143 | printf("snatch_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 144 | } |
yuki0701 | 0:6e2abd0956f1 | 145 | if(sel=='2') { |
yuki0701 | 0:6e2abd0956f1 | 146 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 147 | printf("snatch_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 148 | } |
yuki0701 | 0:6e2abd0956f1 | 149 | if(sel=='3') { |
yuki0701 | 0:6e2abd0956f1 | 150 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 151 | printf("pass1_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 152 | } |
yuki0701 | 0:6e2abd0956f1 | 153 | if(sel=='4') { |
yuki0701 | 0:6e2abd0956f1 | 154 | pass1=1; |
yuki0701 | 0:6e2abd0956f1 | 155 | printf("pass1_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 156 | } |
yuki0701 | 0:6e2abd0956f1 | 157 | if(sel=='5') { |
yuki0701 | 0:6e2abd0956f1 | 158 | pass2=0; |
yuki0701 | 0:6e2abd0956f1 | 159 | printf("pass2_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 160 | } |
yuki0701 | 0:6e2abd0956f1 | 161 | if(sel=='6') { |
yuki0701 | 0:6e2abd0956f1 | 162 | pass2=1; |
yuki0701 | 0:6e2abd0956f1 | 163 | printf("pass2_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 164 | } |
yuki0701 | 0:6e2abd0956f1 | 165 | // if(sel=='a') { |
yuki0701 | 0:6e2abd0956f1 | 166 | // //pc.printf("x\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 167 | // //put(); |
yuki0701 | 0:6e2abd0956f1 | 168 | // } |
yuki0701 | 0:6e2abd0956f1 | 169 | } |
yuki0701 | 0:6e2abd0956f1 | 170 | } |
yuki0701 | 0:6e2abd0956f1 | 171 | #endif |
yuki0701 | 0:6e2abd0956f1 | 172 | #ifdef NHK2020_MAIN_MODE |
yuki0701 | 0:6e2abd0956f1 | 173 | can1.frequency(1000000); |
yuki0701 | 0:6e2abd0956f1 | 174 | can_ticker.attach(&can_read,0.01); |
yuki0701 | 0:6e2abd0956f1 | 175 | printf("wait_mode\r\n"); |
aoikoizumi | 2:90b53995cb1f | 176 | //printf("T2 = %d\n\r",T2); |
aoikoizumi | 2:90b53995cb1f | 177 | if(T2 == 0) { |
aoikoizumi | 2:90b53995cb1f | 178 | while(1) { |
aoikoizumi | 2:90b53995cb1f | 179 | char sel=pc.getc(); |
aoikoizumi | 2:90b53995cb1f | 180 | if(sel=='1') { |
aoikoizumi | 2:90b53995cb1f | 181 | snatch=0; |
aoikoizumi | 2:90b53995cb1f | 182 | printf("snatch_off\r\n"); |
aoikoizumi | 2:90b53995cb1f | 183 | } |
aoikoizumi | 2:90b53995cb1f | 184 | if(sel=='2') { |
aoikoizumi | 2:90b53995cb1f | 185 | snatch=1; |
aoikoizumi | 2:90b53995cb1f | 186 | printf("snatch_on\r\n"); |
aoikoizumi | 2:90b53995cb1f | 187 | } |
aoikoizumi | 2:90b53995cb1f | 188 | if(sel=='3') { |
aoikoizumi | 2:90b53995cb1f | 189 | pass1=0; |
aoikoizumi | 2:90b53995cb1f | 190 | printf("pass1_off\r\n"); |
aoikoizumi | 2:90b53995cb1f | 191 | } |
aoikoizumi | 2:90b53995cb1f | 192 | if(sel=='4') { |
aoikoizumi | 2:90b53995cb1f | 193 | pass1=1; |
aoikoizumi | 2:90b53995cb1f | 194 | printf("pass1_on\r\n"); |
aoikoizumi | 2:90b53995cb1f | 195 | } |
aoikoizumi | 2:90b53995cb1f | 196 | if(sel=='5') { |
aoikoizumi | 2:90b53995cb1f | 197 | pass2=0; |
aoikoizumi | 2:90b53995cb1f | 198 | printf("pass2_off\r\n"); |
aoikoizumi | 2:90b53995cb1f | 199 | } |
aoikoizumi | 2:90b53995cb1f | 200 | if(sel=='6') { |
aoikoizumi | 2:90b53995cb1f | 201 | pass2=1; |
aoikoizumi | 2:90b53995cb1f | 202 | printf("pass2_on\r\n"); |
aoikoizumi | 2:90b53995cb1f | 203 | } |
aoikoizumi | 2:90b53995cb1f | 204 | if(sel=='q') { |
aoikoizumi | 2:90b53995cb1f | 205 | printf("end_wait_mode\r\n"); |
aoikoizumi | 2:90b53995cb1f | 206 | T2=1; |
aoikoizumi | 2:90b53995cb1f | 207 | break; |
yuki0701 | 0:6e2abd0956f1 | 208 | |
aoikoizumi | 2:90b53995cb1f | 209 | } |
yuki0701 | 0:6e2abd0956f1 | 210 | } |
yuki0701 | 0:6e2abd0956f1 | 211 | } |
aoikoizumi | 2:90b53995cb1f | 212 | if(T2 == 1) { //スタート位置からボール前まで移動(アーム待機) |
aoikoizumi | 2:90b53995cb1f | 213 | //T2=1; |
yuki0701 | 0:6e2abd0956f1 | 214 | while(1) { |
aoikoizumi | 2:90b53995cb1f | 215 | printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 216 | wait(0.1); |
aoikoizumi | 2:90b53995cb1f | 217 | if(T2 == 2) { |
yuki0701 | 0:6e2abd0956f1 | 218 | break; |
yuki0701 | 0:6e2abd0956f1 | 219 | } |
yuki0701 | 0:6e2abd0956f1 | 220 | } |
yuki0701 | 0:6e2abd0956f1 | 221 | } |
yuki0701 | 0:6e2abd0956f1 | 222 | |
aoikoizumi | 2:90b53995cb1f | 223 | if(T2 == 2) { //ボール掴んで投げる |
yuki0701 | 0:6e2abd0956f1 | 224 | //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); |
aoikoizumi | 1:bc34fdc4e16b | 225 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 226 | tsukami(); |
aoikoizumi | 1:bc34fdc4e16b | 227 | printf("tsukami"); |
aoikoizumi | 1:bc34fdc4e16b | 228 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 229 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 230 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 231 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 232 | put(); |
aoikoizumi | 1:bc34fdc4e16b | 233 | printf("put"); |
aoikoizumi | 1:bc34fdc4e16b | 234 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 235 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 236 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 237 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 238 | top(); |
aoikoizumi | 1:bc34fdc4e16b | 239 | printf("top"); |
aoikoizumi | 1:bc34fdc4e16b | 240 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 241 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 242 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 243 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 244 | //wait(5); |
yuki0701 | 0:6e2abd0956f1 | 245 | |
yuki0701 | 0:6e2abd0956f1 | 246 | T2++; |
yuki0701 | 0:6e2abd0956f1 | 247 | } |
aoikoizumi | 2:90b53995cb1f | 248 | if(T2 == 3) { //スタートゾーンに戻る(アーム待機) |
yuki0701 | 0:6e2abd0956f1 | 249 | while(1) { |
aoikoizumi | 2:90b53995cb1f | 250 | printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 251 | wait(0.1); |
aoikoizumi | 2:90b53995cb1f | 252 | if(T2 == 4) { |
yuki0701 | 0:6e2abd0956f1 | 253 | break; |
yuki0701 | 0:6e2abd0956f1 | 254 | } |
yuki0701 | 0:6e2abd0956f1 | 255 | } |
yuki0701 | 0:6e2abd0956f1 | 256 | } |
yuki0701 | 0:6e2abd0956f1 | 257 | printf("end\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 258 | } |
yuki0701 | 0:6e2abd0956f1 | 259 | #endif |
yuki0701 | 0:6e2abd0956f1 | 260 | } |