ロボステ6期
/
ball_snatch
e
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?
User | Revision | Line number | New 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 | } |