ロボステ6期 / Mbed 2 deprecated ball_snatch

e

Dependencies:   mbed Encoder_

main.cpp@0:a40701dd7f26, 2019-12-13 (annotated)

Committer:
aoikoizumi
Date:
Fri Dec 13 06:49:35 2019 +0000
Revision:
0:a40701dd7f26
e

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:a40701dd7f26 1 #include "mbed.h"
aoikoizumi 0:a40701dd7f26 2 #include "EC.h" //Encoderライブラリをインクルード
aoikoizumi 0:a40701dd7f26 3 #include "SpeedController.h"
aoikoizumi 0:a40701dd7f26 4 #define RESOLUTION 2048//分解能
aoikoizumi 0:a40701dd7f26 5
aoikoizumi 0:a40701dd7f26 6 Ec4multi EC_backdrop(p15,p16,RESOLUTION);
aoikoizumi 0:a40701dd7f26 7 SpeedControl backdrop(p21,p22,50,EC_backdrop);
aoikoizumi 0:a40701dd7f26 8 Serial pc(USBTX,USBRX);
aoikoizumi 0:a40701dd7f26 9 DigitalOut snatch(p8);
aoikoizumi 0:a40701dd7f26 10 DigitalOut pass1(p27);
aoikoizumi 0:a40701dd7f26 11 DigitalOut pass2(p28);
aoikoizumi 0:a40701dd7f26 12 //Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:a40701dd7f26 13 double a=0;//now
aoikoizumi 0:a40701dd7f26 14 double b=0;//target
aoikoizumi 0:a40701dd7f26 15 double turn=0;
aoikoizumi 0:a40701dd7f26 16 void tsukami()
aoikoizumi 0:a40701dd7f26 17 {
aoikoizumi 0:a40701dd7f26 18 b=1.3;
aoikoizumi 0:a40701dd7f26 19 }
aoikoizumi 0:a40701dd7f26 20 void put()
aoikoizumi 0:a40701dd7f26 21 {
aoikoizumi 0:a40701dd7f26 22 b=-0.7;
aoikoizumi 0:a40701dd7f26 23 }
aoikoizumi 0:a40701dd7f26 24 void top()
aoikoizumi 0:a40701dd7f26 25 {
aoikoizumi 0:a40701dd7f26 26 b=0;
aoikoizumi 0:a40701dd7f26 27 }
aoikoizumi 0:a40701dd7f26 28 int main()
aoikoizumi 0:a40701dd7f26 29 {
aoikoizumi 0:a40701dd7f26 30 pc.printf("setting please");
aoikoizumi 0:a40701dd7f26 31 while(1) {
aoikoizumi 0:a40701dd7f26 32 double old_turn=turn;
aoikoizumi 0:a40701dd7f26 33 a=EC_backdrop.getRad();
aoikoizumi 0:a40701dd7f26 34 if(a-b>=0.1) {
aoikoizumi 0:a40701dd7f26 35 turn=0.1;
aoikoizumi 0:a40701dd7f26 36 pc.printf("F");
aoikoizumi 0:a40701dd7f26 37
aoikoizumi 0:a40701dd7f26 38 } else if (a-b>=0.05) {
aoikoizumi 0:a40701dd7f26 39 turn=10*(a-b)*(a-b);
aoikoizumi 0:a40701dd7f26 40 pc.printf("f");
aoikoizumi 0:a40701dd7f26 41 } else if (b-a>=0.1) {
aoikoizumi 0:a40701dd7f26 42 turn=-0.1;
aoikoizumi 0:a40701dd7f26 43 pc.printf("B");
aoikoizumi 0:a40701dd7f26 44 } else if (b-a>=0.05) {
aoikoizumi 0:a40701dd7f26 45 turn=-10*(a-b)*(a-b);
aoikoizumi 0:a40701dd7f26 46 pc.printf("b");
aoikoizumi 0:a40701dd7f26 47 } else {
aoikoizumi 0:a40701dd7f26 48 backdrop.stop();
aoikoizumi 0:a40701dd7f26 49 backdrop.turn(0);
aoikoizumi 0:a40701dd7f26 50 turn=0;
aoikoizumi 0:a40701dd7f26 51 pc.printf("s");
aoikoizumi 0:a40701dd7f26 52 }
aoikoizumi 0:a40701dd7f26 53
aoikoizumi 0:a40701dd7f26 54 if(turn*old_turn<0)turn=0;
aoikoizumi 0:a40701dd7f26 55 backdrop.turn(turn);
aoikoizumi 0:a40701dd7f26 56 pc.printf("%lf",EC_backdrop.getRad());
aoikoizumi 0:a40701dd7f26 57 if(pc.readable()) {
aoikoizumi 0:a40701dd7f26 58 char sel=pc.getc();
aoikoizumi 0:a40701dd7f26 59 if(sel=='z') {
aoikoizumi 0:a40701dd7f26 60 pc.printf("z\r\n");
aoikoizumi 0:a40701dd7f26 61 tsukami();
aoikoizumi 0:a40701dd7f26 62 } else if(sel=='x') {
aoikoizumi 0:a40701dd7f26 63 pc.printf("x\r\n");
aoikoizumi 0:a40701dd7f26 64 put();
aoikoizumi 0:a40701dd7f26 65 } else if(sel=='c') {
aoikoizumi 0:a40701dd7f26 66 pc.printf("c\r\n");
aoikoizumi 0:a40701dd7f26 67 top();
aoikoizumi 0:a40701dd7f26 68 }
aoikoizumi 0:a40701dd7f26 69 /* if(sel=='q') {
aoikoizumi 0:a40701dd7f26 70 printf("\r\n");
aoikoizumi 0:a40701dd7f26 71 if(denjiben==0)denjiben=1;
aoikoizumi 0:a40701dd7f26 72 else denjiben=0;
aoikoizumi 0:a40701dd7f26 73 }*/
aoikoizumi 0:a40701dd7f26 74 if(sel=='1') {
aoikoizumi 0:a40701dd7f26 75 snatch=0;
aoikoizumi 0:a40701dd7f26 76 printf("snatch_off\r\n");
aoikoizumi 0:a40701dd7f26 77 }
aoikoizumi 0:a40701dd7f26 78 if(sel=='2') {
aoikoizumi 0:a40701dd7f26 79 snatch=1;
aoikoizumi 0:a40701dd7f26 80 printf("snatch_on\r\n");
aoikoizumi 0:a40701dd7f26 81 }
aoikoizumi 0:a40701dd7f26 82 if(sel=='3') {
aoikoizumi 0:a40701dd7f26 83 pass1=0;
aoikoizumi 0:a40701dd7f26 84 printf("pass1_off\r\n");
aoikoizumi 0:a40701dd7f26 85 }
aoikoizumi 0:a40701dd7f26 86 if(sel=='4') {
aoikoizumi 0:a40701dd7f26 87 pass1=1;
aoikoizumi 0:a40701dd7f26 88 printf("pass1_on\r\n");
aoikoizumi 0:a40701dd7f26 89 }
aoikoizumi 0:a40701dd7f26 90 if(sel=='5') {
aoikoizumi 0:a40701dd7f26 91 pass2=0;
aoikoizumi 0:a40701dd7f26 92 printf("pass2_off\r\n");
aoikoizumi 0:a40701dd7f26 93 }
aoikoizumi 0:a40701dd7f26 94 if(sel=='6') {
aoikoizumi 0:a40701dd7f26 95 pass2=1;
aoikoizumi 0:a40701dd7f26 96 printf("pass2_on\r\n");
aoikoizumi 0:a40701dd7f26 97 }
aoikoizumi 0:a40701dd7f26 98 // if(sel=='a') {
aoikoizumi 0:a40701dd7f26 99 // //pc.printf("x\r\n");
aoikoizumi 0:a40701dd7f26 100 // //put();
aoikoizumi 0:a40701dd7f26 101 // }
aoikoizumi 0:a40701dd7f26 102 }
aoikoizumi 0:a40701dd7f26 103 }
aoikoizumi 0:a40701dd7f26 104 }