ロボステ6期 / Mbed 2 deprecated NHK2020-arm-sub3

12/20

Dependencies:   mbed Encoder_

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 
12/20

Who changed what in which revision?

UserRevisionLine numberNew 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 }