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

12/20 mainmode3

Dependencies:   mbed Encoder_

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?

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