ケンタ ミヤザキ
/
nhk_2019_master_01
nhk2019_master
main.cpp@1:be4238d819bb, 2019-03-02 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Mar 02 01:11:16 2019 +0000
- Revision:
- 1:be4238d819bb
- Parent:
- 0:9969c88facec
nhk2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenken0721 | 0:9969c88facec | 1 | #include "mbed.h" |
kenken0721 | 0:9969c88facec | 2 | |
kenken0721 | 0:9969c88facec | 3 | Serial pc( USBTX, USBRX); |
kenken0721 | 0:9969c88facec | 4 | CAN can( PA_11, PA_12); |
kenken0721 | 0:9969c88facec | 5 | CANMessage msg; |
kenken0721 | 0:9969c88facec | 6 | DigitalOut led (PA_5); |
kenken0721 | 0:9969c88facec | 7 | DigitalIn start(PB_13); |
kenken0721 | 0:9969c88facec | 8 | |
kenken0721 | 1:be4238d819bb | 9 | |
kenken0721 | 1:be4238d819bb | 10 | typedef union{ |
kenken0721 | 1:be4238d819bb | 11 | float val; |
kenken0721 | 1:be4238d819bb | 12 | char binary[4]; |
kenken0721 | 1:be4238d819bb | 13 | }transFloat; |
kenken0721 | 1:be4238d819bb | 14 | |
kenken0721 | 1:be4238d819bb | 15 | //P,I,D,YPOWER,ZPOWERの順 |
kenken0721 | 1:be4238d819bb | 16 | const float parameter[4][5] = { |
kenken0721 | 1:be4238d819bb | 17 | {0.0006, 0.001, 0.0, 0.25, 0.25}, |
kenken0721 | 1:be4238d819bb | 18 | {0.0006, 0.001, 0.0, 0.30, 0.25}, |
kenken0721 | 1:be4238d819bb | 19 | {0.0006, 0.001, 0.0, 0.25, 0.25}, |
kenken0721 | 1:be4238d819bb | 20 | {0.0006, 0.001, 0.0, 0.25, 0.25}}; |
kenken0721 | 1:be4238d819bb | 21 | |
kenken0721 | 1:be4238d819bb | 22 | |
kenken0721 | 0:9969c88facec | 23 | bool finishFlags[4] = {false, false, false, false}; |
kenken0721 | 1:be4238d819bb | 24 | |
kenken0721 | 0:9969c88facec | 25 | void checkFlags(int id){ |
kenken0721 | 0:9969c88facec | 26 | char data = 10; |
kenken0721 | 0:9969c88facec | 27 | if(!finishFlags[id]){ |
kenken0721 | 0:9969c88facec | 28 | can.write( CANMessage((id + 4),&data,1) ); |
kenken0721 | 0:9969c88facec | 29 | while(1){ |
kenken0721 | 0:9969c88facec | 30 | if(can.read(msg)){ |
kenken0721 | 0:9969c88facec | 31 | break; |
kenken0721 | 0:9969c88facec | 32 | } |
kenken0721 | 0:9969c88facec | 33 | } |
kenken0721 | 1:be4238d819bb | 34 | if(msg.id == id+14){ |
kenken0721 | 0:9969c88facec | 35 | if(msg.data[0] == 0){ |
kenken0721 | 1:be4238d819bb | 36 | finishFlags[msg.id - 14] = false; |
kenken0721 | 0:9969c88facec | 37 | }else{ |
kenken0721 | 1:be4238d819bb | 38 | finishFlags[msg.id - 14] = true; |
kenken0721 | 0:9969c88facec | 39 | } |
kenken0721 | 0:9969c88facec | 40 | } |
kenken0721 | 0:9969c88facec | 41 | } |
kenken0721 | 0:9969c88facec | 42 | } |
kenken0721 | 0:9969c88facec | 43 | |
kenken0721 | 0:9969c88facec | 44 | |
kenken0721 | 0:9969c88facec | 45 | void waitReach(int id){ |
kenken0721 | 0:9969c88facec | 46 | while(1){ |
kenken0721 | 0:9969c88facec | 47 | checkFlags(id); |
kenken0721 | 0:9969c88facec | 48 | pc.printf("%d \n",finishFlags[id]); |
kenken0721 | 0:9969c88facec | 49 | if(finishFlags[id] == true){ |
kenken0721 | 0:9969c88facec | 50 | break; |
kenken0721 | 0:9969c88facec | 51 | } |
kenken0721 | 0:9969c88facec | 52 | } |
kenken0721 | 0:9969c88facec | 53 | } |
kenken0721 | 0:9969c88facec | 54 | |
kenken0721 | 0:9969c88facec | 55 | void allWaitReach(){ |
kenken0721 | 0:9969c88facec | 56 | for(int i=0;i<4;i++){ |
kenken0721 | 0:9969c88facec | 57 | waitReach(i); |
kenken0721 | 0:9969c88facec | 58 | } |
kenken0721 | 0:9969c88facec | 59 | } |
kenken0721 | 0:9969c88facec | 60 | |
kenken0721 | 0:9969c88facec | 61 | void step(int id, int y, bool z){ |
kenken0721 | 0:9969c88facec | 62 | char data[2] = {y,z}; |
kenken0721 | 0:9969c88facec | 63 | can.write(CANMessage(id,data,2) ); |
kenken0721 | 0:9969c88facec | 64 | finishFlags[id] = false; |
kenken0721 | 0:9969c88facec | 65 | wait_ms(2); |
kenken0721 | 0:9969c88facec | 66 | } |
kenken0721 | 0:9969c88facec | 67 | |
kenken0721 | 0:9969c88facec | 68 | int main() { |
kenken0721 | 0:9969c88facec | 69 | wait(1); |
kenken0721 | 0:9969c88facec | 70 | start.mode(PullUp); |
kenken0721 | 0:9969c88facec | 71 | can.frequency(100000); |
kenken0721 | 0:9969c88facec | 72 | led.write(0); |
kenken0721 | 1:be4238d819bb | 73 | |
kenken0721 | 1:be4238d819bb | 74 | |
kenken0721 | 1:be4238d819bb | 75 | |
kenken0721 | 0:9969c88facec | 76 | int count = 0; |
kenken0721 | 0:9969c88facec | 77 | while(1){ |
kenken0721 | 0:9969c88facec | 78 | if(start.read() == 0){ |
kenken0721 | 0:9969c88facec | 79 | count++; |
kenken0721 | 0:9969c88facec | 80 | } |
kenken0721 | 0:9969c88facec | 81 | if(count>= 100){ |
kenken0721 | 0:9969c88facec | 82 | break; |
kenken0721 | 0:9969c88facec | 83 | } |
kenken0721 | 0:9969c88facec | 84 | } |
kenken0721 | 0:9969c88facec | 85 | |
kenken0721 | 0:9969c88facec | 86 | step(0,10,1); |
kenken0721 | 0:9969c88facec | 87 | step(1,10,0); |
kenken0721 | 0:9969c88facec | 88 | step(2,10,0); |
kenken0721 | 0:9969c88facec | 89 | step(3,10,1); |
kenken0721 | 0:9969c88facec | 90 | allWaitReach(); |
kenken0721 | 0:9969c88facec | 91 | |
kenken0721 | 0:9969c88facec | 92 | wait(2.0); |
kenken0721 | 0:9969c88facec | 93 | while (true) { |
kenken0721 | 0:9969c88facec | 94 | /* |
kenken0721 | 0:9969c88facec | 95 | //led.write(1); |
kenken0721 | 1:be4238d819bb | 96 | step(0,20,1); |
kenken0721 | 1:be4238d819bb | 97 | step(1,200,1); |
kenken0721 | 1:be4238d819bb | 98 | step(2,20,0); |
kenken0721 | 1:be4238d819bb | 99 | step(3,200,0); |
kenken0721 | 1:be4238d819bb | 100 | waitReach(0); |
kenken0721 | 1:be4238d819bb | 101 | waitReach(1); |
kenken0721 | 1:be4238d819bb | 102 | waitReach(2); |
kenken0721 | 1:be4238d819bb | 103 | waitReach(3); |
kenken0721 | 0:9969c88facec | 104 | //pc.printf("ok \n"); |
kenken0721 | 0:9969c88facec | 105 | //wait(3.0); |
kenken0721 | 1:be4238d819bb | 106 | step(0,200,0); |
kenken0721 | 1:be4238d819bb | 107 | step(1,20,0); |
kenken0721 | 1:be4238d819bb | 108 | step(2,200,1); |
kenken0721 | 1:be4238d819bb | 109 | step(3,20,1); |
kenken0721 | 1:be4238d819bb | 110 | |
kenken0721 | 1:be4238d819bb | 111 | waitReach(0); |
kenken0721 | 1:be4238d819bb | 112 | waitReach(1); |
kenken0721 | 1:be4238d819bb | 113 | waitReach(2); |
kenken0721 | 1:be4238d819bb | 114 | waitReach(3); |
kenken0721 | 1:be4238d819bb | 115 | */ |
kenken0721 | 0:9969c88facec | 116 | //pc.printf("ok \n"); |
kenken0721 | 1:be4238d819bb | 117 | |
kenken0721 | 0:9969c88facec | 118 | |
kenken0721 | 0:9969c88facec | 119 | step(1,200,0); |
kenken0721 | 0:9969c88facec | 120 | step(2,200,0); |
kenken0721 | 0:9969c88facec | 121 | waitReach(1); |
kenken0721 | 0:9969c88facec | 122 | waitReach(2); |
kenken0721 | 0:9969c88facec | 123 | |
kenken0721 | 0:9969c88facec | 124 | step(0,10,0); |
kenken0721 | 0:9969c88facec | 125 | step(1,200,1); |
kenken0721 | 0:9969c88facec | 126 | step(2,200,1); |
kenken0721 | 0:9969c88facec | 127 | step(3,10,0); |
kenken0721 | 1:be4238d819bb | 128 | waitReach(0); |
kenken0721 | 1:be4238d819bb | 129 | waitReach(1); |
kenken0721 | 1:be4238d819bb | 130 | waitReach(2); |
kenken0721 | 1:be4238d819bb | 131 | waitReach(3); |
kenken0721 | 0:9969c88facec | 132 | |
kenken0721 | 0:9969c88facec | 133 | step(1,10,1); |
kenken0721 | 0:9969c88facec | 134 | step(2,10,1); |
kenken0721 | 0:9969c88facec | 135 | waitReach(1); |
kenken0721 | 0:9969c88facec | 136 | waitReach(2); |
kenken0721 | 0:9969c88facec | 137 | |
kenken0721 | 0:9969c88facec | 138 | step(0,200,0); |
kenken0721 | 0:9969c88facec | 139 | step(3,200,0); |
kenken0721 | 0:9969c88facec | 140 | waitReach(0); |
kenken0721 | 0:9969c88facec | 141 | waitReach(3); |
kenken0721 | 0:9969c88facec | 142 | |
kenken0721 | 0:9969c88facec | 143 | step(0,200,1); |
kenken0721 | 0:9969c88facec | 144 | step(1,10,0); |
kenken0721 | 0:9969c88facec | 145 | step(2,10,0); |
kenken0721 | 0:9969c88facec | 146 | step(3,200,1); |
kenken0721 | 1:be4238d819bb | 147 | waitReach(0); |
kenken0721 | 1:be4238d819bb | 148 | waitReach(1); |
kenken0721 | 1:be4238d819bb | 149 | waitReach(2); |
kenken0721 | 1:be4238d819bb | 150 | waitReach(3); |
kenken0721 | 0:9969c88facec | 151 | |
kenken0721 | 0:9969c88facec | 152 | step(0,10,1); |
kenken0721 | 0:9969c88facec | 153 | step(3,10,1); |
kenken0721 | 0:9969c88facec | 154 | waitReach(0); |
kenken0721 | 0:9969c88facec | 155 | waitReach(3); |
kenken0721 | 0:9969c88facec | 156 | |
kenken0721 | 0:9969c88facec | 157 | |
kenken0721 | 0:9969c88facec | 158 | //wait(3.0); |
kenken0721 | 0:9969c88facec | 159 | //pc.printf("%d \t",val); |
kenken0721 | 0:9969c88facec | 160 | |
kenken0721 | 0:9969c88facec | 161 | } |
kenken0721 | 0:9969c88facec | 162 | } |