ケンタ ミヤザキ / Mbed 2 deprecated nhk_2019_slave_01

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?

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