nhk2019
Dependencies: mbed mbed-STM32F103C8T6
main.cpp@1:bb93f8b91191, 2019-03-02 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Mar 02 01:10:45 2019 +0000
- Revision:
- 1:bb93f8b91191
- Parent:
- 0:333ed669a96a
nhk2019;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenken0721 | 0:333ed669a96a | 1 | #include "mbed.h" |
kenken0721 | 0:333ed669a96a | 2 | #include "stm32f103c8t6.h" |
kenken0721 | 1:bb93f8b91191 | 3 | const int ID = 3; |
kenken0721 | 0:333ed669a96a | 4 | |
kenken0721 | 0:333ed669a96a | 5 | CANMessage msg; |
kenken0721 | 0:333ed669a96a | 6 | CAN Reseive (PA_11, PA_12); |
kenken0721 | 0:333ed669a96a | 7 | InterruptIn A(PB_0);//A0 |
kenken0721 | 0:333ed669a96a | 8 | DigitalIn B(PB_1);//A1 |
kenken0721 | 0:333ed669a96a | 9 | Ticker pid; |
kenken0721 | 0:333ed669a96a | 10 | |
kenken0721 | 0:333ed669a96a | 11 | /*-----モーター---*/ |
kenken0721 | 0:333ed669a96a | 12 | //z軸 |
kenken0721 | 0:333ed669a96a | 13 | PwmOut pwmZ(PB_3);//D3 |
kenken0721 | 0:333ed669a96a | 14 | DigitalOut z1(PB_4);//D4 |
kenken0721 | 0:333ed669a96a | 15 | DigitalOut z2(PA_15);//D5 |
kenken0721 | 0:333ed669a96a | 16 | //y軸 |
kenken0721 | 0:333ed669a96a | 17 | PwmOut pwmY(PA_8);//D3 |
kenken0721 | 0:333ed669a96a | 18 | DigitalOut y1(PB_14);//D4 |
kenken0721 | 0:333ed669a96a | 19 | DigitalOut y2(PB_15);//D5 |
kenken0721 | 0:333ed669a96a | 20 | |
kenken0721 | 0:333ed669a96a | 21 | |
kenken0721 | 0:333ed669a96a | 22 | DigitalOut led(PC_13); |
kenken0721 | 0:333ed669a96a | 23 | |
kenken0721 | 0:333ed669a96a | 24 | DigitalIn switch1(PA_1);//A2 |
kenken0721 | 0:333ed669a96a | 25 | DigitalIn switch2(PA_2);//A2 |
kenken0721 | 0:333ed669a96a | 26 | DigitalIn switch3(PA_3);//A2 |
kenken0721 | 0:333ed669a96a | 27 | |
kenken0721 | 0:333ed669a96a | 28 | /*-----PID変数----*/ |
kenken0721 | 1:bb93f8b91191 | 29 | const float Kp = 0.0006; |
kenken0721 | 1:bb93f8b91191 | 30 | const float Ki = 0.001; |
kenken0721 | 0:333ed669a96a | 31 | const float Kd = 0.0; |
kenken0721 | 0:333ed669a96a | 32 | float p,i,d; |
kenken0721 | 0:333ed669a96a | 33 | float prep = 0.0; |
kenken0721 | 0:333ed669a96a | 34 | float goal = 0.0;//目標回転数 |
kenken0721 | 0:333ed669a96a | 35 | |
kenken0721 | 0:333ed669a96a | 36 | |
kenken0721 | 0:333ed669a96a | 37 | long Count = 0; |
kenken0721 | 0:333ed669a96a | 38 | float val = 0.0; |
kenken0721 | 0:333ed669a96a | 39 | const float interval = 0.002; |
kenken0721 | 1:bb93f8b91191 | 40 | const float basisPowY = 0.25; |
kenken0721 | 1:bb93f8b91191 | 41 | const float basisPowZ = 0.25; |
kenken0721 | 0:333ed669a96a | 42 | |
kenken0721 | 0:333ed669a96a | 43 | int ymode = 0; |
kenken0721 | 0:333ed669a96a | 44 | |
kenken0721 | 0:333ed669a96a | 45 | bool finishFlagY = false; |
kenken0721 | 0:333ed669a96a | 46 | bool finishFlagZ = false; |
kenken0721 | 0:333ed669a96a | 47 | |
kenken0721 | 0:333ed669a96a | 48 | /*----受信割り込み---*/ |
kenken0721 | 0:333ed669a96a | 49 | void inter(){ |
kenken0721 | 0:333ed669a96a | 50 | Reseive.read(msg); |
kenken0721 | 0:333ed669a96a | 51 | if(msg.id == ID){ |
kenken0721 | 0:333ed669a96a | 52 | finishFlagY = false; |
kenken0721 | 0:333ed669a96a | 53 | finishFlagZ = false; |
kenken0721 | 1:bb93f8b91191 | 54 | int z = int(msg.data[0]); |
kenken0721 | 1:bb93f8b91191 | 55 | int y = int(msg.data[1]); |
kenken0721 | 1:bb93f8b91191 | 56 | goal = float(-1*z); |
kenken0721 | 1:bb93f8b91191 | 57 | ymode = y; |
kenken0721 | 0:333ed669a96a | 58 | }else if(msg.id == (ID+4)){//フラッグチェック用 |
kenken0721 | 0:333ed669a96a | 59 | char p; |
kenken0721 | 1:bb93f8b91191 | 60 | if((finishFlagY == 1) && (finishFlagZ == 1)){ |
kenken0721 | 0:333ed669a96a | 61 | p = 1; |
kenken0721 | 0:333ed669a96a | 62 | }else{ |
kenken0721 | 0:333ed669a96a | 63 | p = 0; |
kenken0721 | 0:333ed669a96a | 64 | } |
kenken0721 | 1:bb93f8b91191 | 65 | Reseive.write( CANMessage( ID+14, &p, 1) ); |
kenken0721 | 1:bb93f8b91191 | 66 | //wait_ms(5); |
kenken0721 | 0:333ed669a96a | 67 | } |
kenken0721 | 0:333ed669a96a | 68 | } |
kenken0721 | 0:333ed669a96a | 69 | |
kenken0721 | 0:333ed669a96a | 70 | void upZ(){ |
kenken0721 | 0:333ed669a96a | 71 | if(ID == 0 || ID == 3){ |
kenken0721 | 0:333ed669a96a | 72 | z1 = 0; |
kenken0721 | 0:333ed669a96a | 73 | z2 = 1; |
kenken0721 | 0:333ed669a96a | 74 | }else{ |
kenken0721 | 0:333ed669a96a | 75 | z1 = 1; |
kenken0721 | 0:333ed669a96a | 76 | z2 = 0; |
kenken0721 | 0:333ed669a96a | 77 | } |
kenken0721 | 0:333ed669a96a | 78 | } |
kenken0721 | 0:333ed669a96a | 79 | |
kenken0721 | 0:333ed669a96a | 80 | void downZ(){ |
kenken0721 | 0:333ed669a96a | 81 | if(ID == 0 || ID == 3){ |
kenken0721 | 0:333ed669a96a | 82 | z1 = 1; |
kenken0721 | 0:333ed669a96a | 83 | z2 = 0; |
kenken0721 | 0:333ed669a96a | 84 | }else{ |
kenken0721 | 0:333ed669a96a | 85 | z1 = 0; |
kenken0721 | 0:333ed669a96a | 86 | z2 = 1; |
kenken0721 | 0:333ed669a96a | 87 | } |
kenken0721 | 0:333ed669a96a | 88 | } |
kenken0721 | 0:333ed669a96a | 89 | |
kenken0721 | 0:333ed669a96a | 90 | void forwardY(){ |
kenken0721 | 0:333ed669a96a | 91 | if(ID == 0 || ID == 2){ |
kenken0721 | 0:333ed669a96a | 92 | y1 = 1; |
kenken0721 | 0:333ed669a96a | 93 | y2 = 0; |
kenken0721 | 0:333ed669a96a | 94 | }else{ |
kenken0721 | 0:333ed669a96a | 95 | y1 = 0; |
kenken0721 | 0:333ed669a96a | 96 | y2 = 1; |
kenken0721 | 0:333ed669a96a | 97 | } |
kenken0721 | 0:333ed669a96a | 98 | } |
kenken0721 | 0:333ed669a96a | 99 | void backY(){ |
kenken0721 | 0:333ed669a96a | 100 | if(ID == 0 || ID == 2){ |
kenken0721 | 0:333ed669a96a | 101 | y1 = 0; |
kenken0721 | 0:333ed669a96a | 102 | y2 = 1; |
kenken0721 | 0:333ed669a96a | 103 | }else{ |
kenken0721 | 0:333ed669a96a | 104 | y1 = 1; |
kenken0721 | 0:333ed669a96a | 105 | y2 = 0; |
kenken0721 | 0:333ed669a96a | 106 | } |
kenken0721 | 0:333ed669a96a | 107 | } |
kenken0721 | 0:333ed669a96a | 108 | void stopZ(){ |
kenken0721 | 0:333ed669a96a | 109 | z1 = 1; |
kenken0721 | 0:333ed669a96a | 110 | z2 = 1; |
kenken0721 | 0:333ed669a96a | 111 | } |
kenken0721 | 0:333ed669a96a | 112 | void stopY(){ |
kenken0721 | 0:333ed669a96a | 113 | y1 = 1; |
kenken0721 | 0:333ed669a96a | 114 | y2 = 1; |
kenken0721 | 0:333ed669a96a | 115 | } |
kenken0721 | 0:333ed669a96a | 116 | void Arise(){ |
kenken0721 | 0:333ed669a96a | 117 | if(int(B) == 0){ |
kenken0721 | 0:333ed669a96a | 118 | Count++; |
kenken0721 | 0:333ed669a96a | 119 | }else{ |
kenken0721 | 0:333ed669a96a | 120 | Count--; |
kenken0721 | 0:333ed669a96a | 121 | } |
kenken0721 | 0:333ed669a96a | 122 | |
kenken0721 | 0:333ed669a96a | 123 | } |
kenken0721 | 0:333ed669a96a | 124 | |
kenken0721 | 0:333ed669a96a | 125 | void Afall(){ |
kenken0721 | 0:333ed669a96a | 126 | if(int(B) == 0){ |
kenken0721 | 0:333ed669a96a | 127 | Count--; |
kenken0721 | 0:333ed669a96a | 128 | }else{ |
kenken0721 | 0:333ed669a96a | 129 | Count++; |
kenken0721 | 0:333ed669a96a | 130 | } |
kenken0721 | 0:333ed669a96a | 131 | |
kenken0721 | 0:333ed669a96a | 132 | } |
kenken0721 | 0:333ed669a96a | 133 | |
kenken0721 | 0:333ed669a96a | 134 | void Update(){ |
kenken0721 | 0:333ed669a96a | 135 | long now = Count; |
kenken0721 | 0:333ed669a96a | 136 | if(ID == 1 || ID == 2){ |
kenken0721 | 0:333ed669a96a | 137 | now = now * -1; |
kenken0721 | 0:333ed669a96a | 138 | } |
kenken0721 | 0:333ed669a96a | 139 | p = goal - float(now);//比例 |
kenken0721 | 0:333ed669a96a | 140 | i += p * interval;//積分 |
kenken0721 | 0:333ed669a96a | 141 | d = (p - prep)/interval;//微分 |
kenken0721 | 0:333ed669a96a | 142 | prep = p; |
kenken0721 | 0:333ed669a96a | 143 | float output = (Kp*p + Ki*i + Kd*d); |
kenken0721 | 0:333ed669a96a | 144 | if(output > 0){ |
kenken0721 | 0:333ed669a96a | 145 | downZ(); |
kenken0721 | 0:333ed669a96a | 146 | if(switch1.read() == 0){ |
kenken0721 | 0:333ed669a96a | 147 | stopZ(); |
kenken0721 | 0:333ed669a96a | 148 | output = 0; |
kenken0721 | 1:bb93f8b91191 | 149 | //Count = 0; |
kenken0721 | 0:333ed669a96a | 150 | } |
kenken0721 | 0:333ed669a96a | 151 | }else if(output < 0){ |
kenken0721 | 0:333ed669a96a | 152 | upZ(); |
kenken0721 | 1:bb93f8b91191 | 153 | led = 0; |
kenken0721 | 0:333ed669a96a | 154 | }else{ |
kenken0721 | 0:333ed669a96a | 155 | stopZ(); |
kenken0721 | 0:333ed669a96a | 156 | output = 0; |
kenken0721 | 0:333ed669a96a | 157 | finishFlagZ = true; |
kenken0721 | 1:bb93f8b91191 | 158 | |
kenken0721 | 0:333ed669a96a | 159 | } |
kenken0721 | 0:333ed669a96a | 160 | if(output > (1 - basisPowZ)){ |
kenken0721 | 0:333ed669a96a | 161 | output = 1 - basisPowZ; |
kenken0721 | 0:333ed669a96a | 162 | }else if(output < (basisPowZ - 1)){ |
kenken0721 | 0:333ed669a96a | 163 | output = basisPowZ -1; |
kenken0721 | 0:333ed669a96a | 164 | } |
kenken0721 | 0:333ed669a96a | 165 | if(p <= 3 && p >= -3){ |
kenken0721 | 0:333ed669a96a | 166 | stopZ(); |
kenken0721 | 0:333ed669a96a | 167 | output = 0; |
kenken0721 | 0:333ed669a96a | 168 | finishFlagZ = true; |
kenken0721 | 1:bb93f8b91191 | 169 | }else{ |
kenken0721 | 1:bb93f8b91191 | 170 | //finishFlagZ = false; |
kenken0721 | 0:333ed669a96a | 171 | } |
kenken0721 | 0:333ed669a96a | 172 | output = abs(output); |
kenken0721 | 0:333ed669a96a | 173 | pwmZ = basisPowZ + output; |
kenken0721 | 0:333ed669a96a | 174 | } |
kenken0721 | 0:333ed669a96a | 175 | |
kenken0721 | 0:333ed669a96a | 176 | int main(){ |
kenken0721 | 0:333ed669a96a | 177 | led = 1; |
kenken0721 | 0:333ed669a96a | 178 | Reseive.frequency(100000); |
kenken0721 | 0:333ed669a96a | 179 | Reseive.attach( &inter); |
kenken0721 | 0:333ed669a96a | 180 | switch1.mode(PullDown); |
kenken0721 | 0:333ed669a96a | 181 | switch2.mode(PullDown); |
kenken0721 | 0:333ed669a96a | 182 | switch3.mode(PullDown); |
kenken0721 | 0:333ed669a96a | 183 | pwmZ.period_us(50); |
kenken0721 | 0:333ed669a96a | 184 | pwmY.period_us(50); |
kenken0721 | 0:333ed669a96a | 185 | |
kenken0721 | 0:333ed669a96a | 186 | //初期位置 |
kenken0721 | 0:333ed669a96a | 187 | pwmZ = basisPowZ; |
kenken0721 | 0:333ed669a96a | 188 | downZ(); |
kenken0721 | 0:333ed669a96a | 189 | pwmY = basisPowY; |
kenken0721 | 0:333ed669a96a | 190 | stopY(); |
kenken0721 | 0:333ed669a96a | 191 | while(1){ |
kenken0721 | 0:333ed669a96a | 192 | if(switch1.read() == 0){ |
kenken0721 | 0:333ed669a96a | 193 | stopZ(); |
kenken0721 | 0:333ed669a96a | 194 | break; |
kenken0721 | 0:333ed669a96a | 195 | } |
kenken0721 | 0:333ed669a96a | 196 | } |
kenken0721 | 0:333ed669a96a | 197 | pid.attach(&Update,interval); |
kenken0721 | 0:333ed669a96a | 198 | A.mode(PullUp); |
kenken0721 | 0:333ed669a96a | 199 | B.mode(PullUp); |
kenken0721 | 0:333ed669a96a | 200 | A.rise(&Arise); |
kenken0721 | 0:333ed669a96a | 201 | A.fall(&Afall); |
kenken0721 | 0:333ed669a96a | 202 | //led = 0; |
kenken0721 | 0:333ed669a96a | 203 | while (true) { |
kenken0721 | 0:333ed669a96a | 204 | if(finishFlagY == false){ |
kenken0721 | 0:333ed669a96a | 205 | if(ymode == 1){ |
kenken0721 | 0:333ed669a96a | 206 | if(switch2.read() == 0){ |
kenken0721 | 0:333ed669a96a | 207 | stopY(); |
kenken0721 | 0:333ed669a96a | 208 | finishFlagY = true; |
kenken0721 | 0:333ed669a96a | 209 | }else{//前へ |
kenken0721 | 0:333ed669a96a | 210 | forwardY(); |
kenken0721 | 1:bb93f8b91191 | 211 | //finishFlagY = false; |
kenken0721 | 0:333ed669a96a | 212 | } |
kenken0721 | 0:333ed669a96a | 213 | }else{ |
kenken0721 | 0:333ed669a96a | 214 | if(switch3.read() == 0){ |
kenken0721 | 0:333ed669a96a | 215 | stopY(); |
kenken0721 | 0:333ed669a96a | 216 | finishFlagY = true; |
kenken0721 | 0:333ed669a96a | 217 | }else{//後ろへ |
kenken0721 | 0:333ed669a96a | 218 | backY(); |
kenken0721 | 1:bb93f8b91191 | 219 | //finishFlagY = false; |
kenken0721 | 0:333ed669a96a | 220 | } |
kenken0721 | 0:333ed669a96a | 221 | } |
kenken0721 | 0:333ed669a96a | 222 | }else{ |
kenken0721 | 0:333ed669a96a | 223 | stopY(); |
kenken0721 | 1:bb93f8b91191 | 224 | } |
kenken0721 | 1:bb93f8b91191 | 225 | if(switch1.read() == 0){ |
kenken0721 | 1:bb93f8b91191 | 226 | Count = 0; |
kenken0721 | 1:bb93f8b91191 | 227 | } |
kenken0721 | 0:333ed669a96a | 228 | } |
kenken0721 | 0:333ed669a96a | 229 | } |