勇紀 有岡 / Mbed 2 deprecated NHK2020-arm-sub

arm

Dependencies:   mbed Encoder_

main.cpp@0:6e2abd0956f1, 2019-12-14 (annotated)

Committer:
yuki0701
Date:
Sat Dec 14 12:42:13 2019 +0000
Revision:
0:6e2abd0956f1
arm;

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