ロボステ6期
/
NHK2020-arm-sub3
12/20
main.cpp@1:bc34fdc4e16b, 2019-12-18 (annotated)
- Committer:
- aoikoizumi
- Date:
- Wed Dec 18 02:33:45 2019 +0000
- Revision:
- 1:bc34fdc4e16b
- Parent:
- 0:6e2abd0956f1
- Child:
- 2:90b53995cb1f
f
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 |
yuki0701 | 0:6e2abd0956f1 | 122 | |
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"); |
yuki0701 | 0:6e2abd0956f1 | 176 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 177 | char sel=pc.getc(); |
yuki0701 | 0:6e2abd0956f1 | 178 | if(sel=='1') { |
yuki0701 | 0:6e2abd0956f1 | 179 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 180 | printf("snatch_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 181 | } |
yuki0701 | 0:6e2abd0956f1 | 182 | if(sel=='2') { |
yuki0701 | 0:6e2abd0956f1 | 183 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 184 | printf("snatch_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 185 | } |
yuki0701 | 0:6e2abd0956f1 | 186 | if(sel=='3') { |
yuki0701 | 0:6e2abd0956f1 | 187 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 188 | printf("pass1_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 189 | } |
yuki0701 | 0:6e2abd0956f1 | 190 | if(sel=='4') { |
yuki0701 | 0:6e2abd0956f1 | 191 | pass1=1; |
yuki0701 | 0:6e2abd0956f1 | 192 | printf("pass1_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 193 | } |
yuki0701 | 0:6e2abd0956f1 | 194 | if(sel=='5') { |
yuki0701 | 0:6e2abd0956f1 | 195 | pass2=0; |
yuki0701 | 0:6e2abd0956f1 | 196 | printf("pass2_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 197 | } |
yuki0701 | 0:6e2abd0956f1 | 198 | if(sel=='6') { |
yuki0701 | 0:6e2abd0956f1 | 199 | pass2=1; |
yuki0701 | 0:6e2abd0956f1 | 200 | printf("pass2_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 201 | } |
yuki0701 | 0:6e2abd0956f1 | 202 | if(sel=='q') { |
yuki0701 | 0:6e2abd0956f1 | 203 | printf("end_wait_mode\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 204 | break; |
yuki0701 | 0:6e2abd0956f1 | 205 | |
yuki0701 | 0:6e2abd0956f1 | 206 | } |
yuki0701 | 0:6e2abd0956f1 | 207 | } |
yuki0701 | 0:6e2abd0956f1 | 208 | |
yuki0701 | 0:6e2abd0956f1 | 209 | if(T2 == 0) { //スタート位置からボール前まで移動(アーム待機) |
yuki0701 | 0:6e2abd0956f1 | 210 | T2=1; |
yuki0701 | 0:6e2abd0956f1 | 211 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 212 | //printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 213 | wait(0.1); |
yuki0701 | 0:6e2abd0956f1 | 214 | if(T2 == 1) { |
yuki0701 | 0:6e2abd0956f1 | 215 | break; |
yuki0701 | 0:6e2abd0956f1 | 216 | } |
yuki0701 | 0:6e2abd0956f1 | 217 | } |
yuki0701 | 0:6e2abd0956f1 | 218 | } |
yuki0701 | 0:6e2abd0956f1 | 219 | |
yuki0701 | 0:6e2abd0956f1 | 220 | if(T2 == 1) { //ボール掴んで投げる |
yuki0701 | 0:6e2abd0956f1 | 221 | //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); |
aoikoizumi | 1:bc34fdc4e16b | 222 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 223 | tsukami(); |
aoikoizumi | 1:bc34fdc4e16b | 224 | printf("tsukami"); |
aoikoizumi | 1:bc34fdc4e16b | 225 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 226 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 227 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 228 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 229 | put(); |
aoikoizumi | 1:bc34fdc4e16b | 230 | printf("put"); |
aoikoizumi | 1:bc34fdc4e16b | 231 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 232 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 233 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 234 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 235 | top(); |
aoikoizumi | 1:bc34fdc4e16b | 236 | printf("top"); |
aoikoizumi | 1:bc34fdc4e16b | 237 | move_motor(); |
yuki0701 | 0:6e2abd0956f1 | 238 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 239 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 240 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 241 | //wait(5); |
yuki0701 | 0:6e2abd0956f1 | 242 | |
yuki0701 | 0:6e2abd0956f1 | 243 | T2++; |
yuki0701 | 0:6e2abd0956f1 | 244 | } |
yuki0701 | 0:6e2abd0956f1 | 245 | if(T2 == 2) { //スタートゾーンに戻る(アーム待機) |
yuki0701 | 0:6e2abd0956f1 | 246 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 247 | //printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 248 | wait(0.1); |
yuki0701 | 0:6e2abd0956f1 | 249 | if(T2 == 1) { |
yuki0701 | 0:6e2abd0956f1 | 250 | break; |
yuki0701 | 0:6e2abd0956f1 | 251 | } |
yuki0701 | 0:6e2abd0956f1 | 252 | } |
yuki0701 | 0:6e2abd0956f1 | 253 | } |
yuki0701 | 0:6e2abd0956f1 | 254 | printf("end\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 255 | } |
yuki0701 | 0:6e2abd0956f1 | 256 | #endif |
yuki0701 | 0:6e2abd0956f1 | 257 | } |