ロボステ6期
/
NHK2020-arm-sub3
12/20
main.cpp@0:6e2abd0956f1, 2019-12-14 (annotated)
- Committer:
- yuki0701
- Date:
- Sat Dec 14 12:42:13 2019 +0000
- Revision:
- 0:6e2abd0956f1
- Child:
- 1:bc34fdc4e16b
arm;
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 | } |
yuki0701 | 0:6e2abd0956f1 | 40 | void move() |
yuki0701 | 0:6e2abd0956f1 | 41 | { |
yuki0701 | 0:6e2abd0956f1 | 42 | double old_turn=turn; |
yuki0701 | 0:6e2abd0956f1 | 43 | a=EC_backdrop.getRad(); |
yuki0701 | 0:6e2abd0956f1 | 44 | if(a-b>=0.1) { |
yuki0701 | 0:6e2abd0956f1 | 45 | turn=0.1; |
yuki0701 | 0:6e2abd0956f1 | 46 | //pc.printf("F"); |
yuki0701 | 0:6e2abd0956f1 | 47 | |
yuki0701 | 0:6e2abd0956f1 | 48 | } else if (a-b>=0.05) { |
yuki0701 | 0:6e2abd0956f1 | 49 | turn=10*(a-b)*(a-b); |
yuki0701 | 0:6e2abd0956f1 | 50 | //pc.printf("f"); |
yuki0701 | 0:6e2abd0956f1 | 51 | } else if (b-a>=0.1) { |
yuki0701 | 0:6e2abd0956f1 | 52 | turn=-0.1; |
yuki0701 | 0:6e2abd0956f1 | 53 | //pc.printf("B"); |
yuki0701 | 0:6e2abd0956f1 | 54 | } else if (b-a>=0.05) { |
yuki0701 | 0:6e2abd0956f1 | 55 | turn=-10*(a-b)*(a-b); |
yuki0701 | 0:6e2abd0956f1 | 56 | //pc.printf("b"); |
yuki0701 | 0:6e2abd0956f1 | 57 | } else { |
yuki0701 | 0:6e2abd0956f1 | 58 | backdrop.stop(); |
yuki0701 | 0:6e2abd0956f1 | 59 | backdrop.turn(0); |
yuki0701 | 0:6e2abd0956f1 | 60 | turn=0; |
yuki0701 | 0:6e2abd0956f1 | 61 | //break; |
yuki0701 | 0:6e2abd0956f1 | 62 | //pc.printf("s"); |
yuki0701 | 0:6e2abd0956f1 | 63 | } |
yuki0701 | 0:6e2abd0956f1 | 64 | if(turn*old_turn<0)turn=0; |
yuki0701 | 0:6e2abd0956f1 | 65 | backdrop.turn(turn); |
yuki0701 | 0:6e2abd0956f1 | 66 | } |
yuki0701 | 0:6e2abd0956f1 | 67 | |
yuki0701 | 0:6e2abd0956f1 | 68 | int q = 0; |
yuki0701 | 0:6e2abd0956f1 | 69 | |
yuki0701 | 0:6e2abd0956f1 | 70 | void can_read() |
yuki0701 | 0:6e2abd0956f1 | 71 | { |
yuki0701 | 0:6e2abd0956f1 | 72 | |
yuki0701 | 0:6e2abd0956f1 | 73 | |
yuki0701 | 0:6e2abd0956f1 | 74 | CANMessage msg; |
yuki0701 | 0:6e2abd0956f1 | 75 | |
yuki0701 | 0:6e2abd0956f1 | 76 | if(can1.read(msg)) { |
yuki0701 | 0:6e2abd0956f1 | 77 | |
yuki0701 | 0:6e2abd0956f1 | 78 | if(msg.id == 1) { |
yuki0701 | 0:6e2abd0956f1 | 79 | t2_r = msg.data[0]; |
yuki0701 | 0:6e2abd0956f1 | 80 | //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 81 | } else { |
yuki0701 | 0:6e2abd0956f1 | 82 | if(q > 100) { |
yuki0701 | 0:6e2abd0956f1 | 83 | //printf("id1 fale\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 84 | q = 0; |
yuki0701 | 0:6e2abd0956f1 | 85 | } |
yuki0701 | 0:6e2abd0956f1 | 86 | q++; |
yuki0701 | 0:6e2abd0956f1 | 87 | } |
yuki0701 | 0:6e2abd0956f1 | 88 | |
yuki0701 | 0:6e2abd0956f1 | 89 | } else { |
yuki0701 | 0:6e2abd0956f1 | 90 | if(k > 100) { |
yuki0701 | 0:6e2abd0956f1 | 91 | //printf("arm fale\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 92 | k = 0; |
yuki0701 | 0:6e2abd0956f1 | 93 | } |
yuki0701 | 0:6e2abd0956f1 | 94 | k++; |
yuki0701 | 0:6e2abd0956f1 | 95 | } |
yuki0701 | 0:6e2abd0956f1 | 96 | |
yuki0701 | 0:6e2abd0956f1 | 97 | t2[0] = T2; |
yuki0701 | 0:6e2abd0956f1 | 98 | |
yuki0701 | 0:6e2abd0956f1 | 99 | if(can1.write(CANMessage(2,t2,1))) { |
yuki0701 | 0:6e2abd0956f1 | 100 | } |
yuki0701 | 0:6e2abd0956f1 | 101 | |
yuki0701 | 0:6e2abd0956f1 | 102 | if(t2_r > T2) { |
yuki0701 | 0:6e2abd0956f1 | 103 | T2 = t2_r; |
yuki0701 | 0:6e2abd0956f1 | 104 | } |
yuki0701 | 0:6e2abd0956f1 | 105 | |
yuki0701 | 0:6e2abd0956f1 | 106 | |
yuki0701 | 0:6e2abd0956f1 | 107 | } |
yuki0701 | 0:6e2abd0956f1 | 108 | |
yuki0701 | 0:6e2abd0956f1 | 109 | |
yuki0701 | 0:6e2abd0956f1 | 110 | int main() |
yuki0701 | 0:6e2abd0956f1 | 111 | { |
yuki0701 | 0:6e2abd0956f1 | 112 | move(); |
yuki0701 | 0:6e2abd0956f1 | 113 | |
yuki0701 | 0:6e2abd0956f1 | 114 | pc.printf("setting please"); |
yuki0701 | 0:6e2abd0956f1 | 115 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 116 | |
yuki0701 | 0:6e2abd0956f1 | 117 | |
yuki0701 | 0:6e2abd0956f1 | 118 | |
yuki0701 | 0:6e2abd0956f1 | 119 | |
yuki0701 | 0:6e2abd0956f1 | 120 | #ifdef NHK2020_TEST_MODE |
yuki0701 | 0:6e2abd0956f1 | 121 | |
yuki0701 | 0:6e2abd0956f1 | 122 | pc.printf("%lf",EC_backdrop.getRad()); |
yuki0701 | 0:6e2abd0956f1 | 123 | if(pc.readable()) { |
yuki0701 | 0:6e2abd0956f1 | 124 | char sel=pc.getc(); |
yuki0701 | 0:6e2abd0956f1 | 125 | if(sel=='z') { |
yuki0701 | 0:6e2abd0956f1 | 126 | pc.printf("z\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 127 | tsukami(); |
yuki0701 | 0:6e2abd0956f1 | 128 | } else if(sel=='x') { |
yuki0701 | 0:6e2abd0956f1 | 129 | pc.printf("x\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 130 | put(); |
yuki0701 | 0:6e2abd0956f1 | 131 | } else if(sel=='c') { |
yuki0701 | 0:6e2abd0956f1 | 132 | pc.printf("c\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 133 | top(); |
yuki0701 | 0:6e2abd0956f1 | 134 | } |
yuki0701 | 0:6e2abd0956f1 | 135 | /* if(sel=='q') { |
yuki0701 | 0:6e2abd0956f1 | 136 | printf("\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 137 | if(denjiben==0)denjiben=1; |
yuki0701 | 0:6e2abd0956f1 | 138 | else denjiben=0; |
yuki0701 | 0:6e2abd0956f1 | 139 | }*/ |
yuki0701 | 0:6e2abd0956f1 | 140 | if(sel=='1') { |
yuki0701 | 0:6e2abd0956f1 | 141 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 142 | printf("snatch_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 143 | } |
yuki0701 | 0:6e2abd0956f1 | 144 | if(sel=='2') { |
yuki0701 | 0:6e2abd0956f1 | 145 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 146 | printf("snatch_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 147 | } |
yuki0701 | 0:6e2abd0956f1 | 148 | if(sel=='3') { |
yuki0701 | 0:6e2abd0956f1 | 149 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 150 | printf("pass1_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 151 | } |
yuki0701 | 0:6e2abd0956f1 | 152 | if(sel=='4') { |
yuki0701 | 0:6e2abd0956f1 | 153 | pass1=1; |
yuki0701 | 0:6e2abd0956f1 | 154 | printf("pass1_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 155 | } |
yuki0701 | 0:6e2abd0956f1 | 156 | if(sel=='5') { |
yuki0701 | 0:6e2abd0956f1 | 157 | pass2=0; |
yuki0701 | 0:6e2abd0956f1 | 158 | printf("pass2_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 159 | } |
yuki0701 | 0:6e2abd0956f1 | 160 | if(sel=='6') { |
yuki0701 | 0:6e2abd0956f1 | 161 | pass2=1; |
yuki0701 | 0:6e2abd0956f1 | 162 | printf("pass2_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 163 | } |
yuki0701 | 0:6e2abd0956f1 | 164 | // if(sel=='a') { |
yuki0701 | 0:6e2abd0956f1 | 165 | // //pc.printf("x\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 166 | // //put(); |
yuki0701 | 0:6e2abd0956f1 | 167 | // } |
yuki0701 | 0:6e2abd0956f1 | 168 | } |
yuki0701 | 0:6e2abd0956f1 | 169 | } |
yuki0701 | 0:6e2abd0956f1 | 170 | #endif |
yuki0701 | 0:6e2abd0956f1 | 171 | #ifdef NHK2020_MAIN_MODE |
yuki0701 | 0:6e2abd0956f1 | 172 | can1.frequency(1000000); |
yuki0701 | 0:6e2abd0956f1 | 173 | can_ticker.attach(&can_read,0.01); |
yuki0701 | 0:6e2abd0956f1 | 174 | printf("wait_mode\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 175 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 176 | char sel=pc.getc(); |
yuki0701 | 0:6e2abd0956f1 | 177 | if(sel=='1') { |
yuki0701 | 0:6e2abd0956f1 | 178 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 179 | printf("snatch_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 180 | } |
yuki0701 | 0:6e2abd0956f1 | 181 | if(sel=='2') { |
yuki0701 | 0:6e2abd0956f1 | 182 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 183 | printf("snatch_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 184 | } |
yuki0701 | 0:6e2abd0956f1 | 185 | if(sel=='3') { |
yuki0701 | 0:6e2abd0956f1 | 186 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 187 | printf("pass1_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 188 | } |
yuki0701 | 0:6e2abd0956f1 | 189 | if(sel=='4') { |
yuki0701 | 0:6e2abd0956f1 | 190 | pass1=1; |
yuki0701 | 0:6e2abd0956f1 | 191 | printf("pass1_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 192 | } |
yuki0701 | 0:6e2abd0956f1 | 193 | if(sel=='5') { |
yuki0701 | 0:6e2abd0956f1 | 194 | pass2=0; |
yuki0701 | 0:6e2abd0956f1 | 195 | printf("pass2_off\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 196 | } |
yuki0701 | 0:6e2abd0956f1 | 197 | if(sel=='6') { |
yuki0701 | 0:6e2abd0956f1 | 198 | pass2=1; |
yuki0701 | 0:6e2abd0956f1 | 199 | printf("pass2_on\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 200 | } |
yuki0701 | 0:6e2abd0956f1 | 201 | if(sel=='q') { |
yuki0701 | 0:6e2abd0956f1 | 202 | printf("end_wait_mode\r\n"); |
yuki0701 | 0:6e2abd0956f1 | 203 | break; |
yuki0701 | 0:6e2abd0956f1 | 204 | |
yuki0701 | 0:6e2abd0956f1 | 205 | } |
yuki0701 | 0:6e2abd0956f1 | 206 | } |
yuki0701 | 0:6e2abd0956f1 | 207 | |
yuki0701 | 0:6e2abd0956f1 | 208 | if(T2 == 0) { //スタート位置からボール前まで移動(アーム待機) |
yuki0701 | 0:6e2abd0956f1 | 209 | T2=1; |
yuki0701 | 0:6e2abd0956f1 | 210 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 211 | //printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 212 | wait(0.1); |
yuki0701 | 0:6e2abd0956f1 | 213 | if(T2 == 1) { |
yuki0701 | 0:6e2abd0956f1 | 214 | break; |
yuki0701 | 0:6e2abd0956f1 | 215 | } |
yuki0701 | 0:6e2abd0956f1 | 216 | } |
yuki0701 | 0:6e2abd0956f1 | 217 | } |
yuki0701 | 0:6e2abd0956f1 | 218 | |
yuki0701 | 0:6e2abd0956f1 | 219 | if(T2 == 1) { //ボール掴んで投げる |
yuki0701 | 0:6e2abd0956f1 | 220 | //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 221 | tsukami(); |
yuki0701 | 0:6e2abd0956f1 | 222 | move(); |
yuki0701 | 0:6e2abd0956f1 | 223 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 224 | snatch=0; |
yuki0701 | 0:6e2abd0956f1 | 225 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 226 | put(); |
yuki0701 | 0:6e2abd0956f1 | 227 | wait(3); |
yuki0701 | 0:6e2abd0956f1 | 228 | snatch=1; |
yuki0701 | 0:6e2abd0956f1 | 229 | wait(1); |
yuki0701 | 0:6e2abd0956f1 | 230 | top(); |
yuki0701 | 0:6e2abd0956f1 | 231 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 232 | pass1=0; |
yuki0701 | 0:6e2abd0956f1 | 233 | wait(5); |
yuki0701 | 0:6e2abd0956f1 | 234 | //wait(5); |
yuki0701 | 0:6e2abd0956f1 | 235 | |
yuki0701 | 0:6e2abd0956f1 | 236 | T2++; |
yuki0701 | 0:6e2abd0956f1 | 237 | } |
yuki0701 | 0:6e2abd0956f1 | 238 | if(T2 == 2) { //スタートゾーンに戻る(アーム待機) |
yuki0701 | 0:6e2abd0956f1 | 239 | while(1) { |
yuki0701 | 0:6e2abd0956f1 | 240 | //printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); |
yuki0701 | 0:6e2abd0956f1 | 241 | wait(0.1); |
yuki0701 | 0:6e2abd0956f1 | 242 | if(T2 == 1) { |
yuki0701 | 0:6e2abd0956f1 | 243 | break; |
yuki0701 | 0:6e2abd0956f1 | 244 | } |
yuki0701 | 0:6e2abd0956f1 | 245 | } |
yuki0701 | 0:6e2abd0956f1 | 246 | } |
yuki0701 | 0:6e2abd0956f1 | 247 | printf("end\n\r"); |
yuki0701 | 0:6e2abd0956f1 | 248 | } |
yuki0701 | 0:6e2abd0956f1 | 249 | #endif |
yuki0701 | 0:6e2abd0956f1 | 250 | } |