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

nhk2019

Dependencies:   mbed mbed-STM32F103C8T6

main.cpp@0:333ed669a96a, 2019-02-24 (annotated)

Committer:
kenken0721
Date:
Sun Feb 24 13:33:47 2019 +0000
Revision:
0:333ed669a96a
Child:
1:bb93f8b91191
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 0:333ed669a96a 3 const int ID = 0;
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 0:333ed669a96a 29 const float Kp = 0.0016;
kenken0721 0:333ed669a96a 30 const float Ki = 0.0;
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 0:333ed669a96a 40 const float basisPowY = 0.2;
kenken0721 0:333ed669a96a 41 const float basisPowZ = 0.2;
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 0:333ed669a96a 54 int y = int(msg.data[0]);
kenken0721 0:333ed669a96a 55 int z = int(msg.data[1]);
kenken0721 0:333ed669a96a 56 goal = float(-1*y);
kenken0721 0:333ed669a96a 57 /*
kenken0721 0:333ed669a96a 58 if(ID == 0 || ID == 3){//右前足、左後足
kenken0721 0:333ed669a96a 59 goal = float(-1*y);
kenken0721 0:333ed669a96a 60 }else{
kenken0721 0:333ed669a96a 61 goal = float(y);
kenken0721 0:333ed669a96a 62 }
kenken0721 0:333ed669a96a 63 */
kenken0721 0:333ed669a96a 64 ymode = z;
kenken0721 0:333ed669a96a 65 }else if(msg.id == (ID+4)){//フラッグチェック用
kenken0721 0:333ed669a96a 66 char p;
kenken0721 0:333ed669a96a 67 if(finishFlagY == 1 && finishFlagZ == 1){
kenken0721 0:333ed669a96a 68 p = 1;
kenken0721 0:333ed669a96a 69 }else{
kenken0721 0:333ed669a96a 70 p = 0;
kenken0721 0:333ed669a96a 71 }
kenken0721 0:333ed669a96a 72 Reseive.write( CANMessage( ID+4, &p, 1) );
kenken0721 0:333ed669a96a 73 }
kenken0721 0:333ed669a96a 74 }
kenken0721 0:333ed669a96a 75
kenken0721 0:333ed669a96a 76 void upZ(){
kenken0721 0:333ed669a96a 77 if(ID == 0 || ID == 3){
kenken0721 0:333ed669a96a 78 z1 = 0;
kenken0721 0:333ed669a96a 79 z2 = 1;
kenken0721 0:333ed669a96a 80 }else{
kenken0721 0:333ed669a96a 81 z1 = 1;
kenken0721 0:333ed669a96a 82 z2 = 0;
kenken0721 0:333ed669a96a 83 }
kenken0721 0:333ed669a96a 84 }
kenken0721 0:333ed669a96a 85
kenken0721 0:333ed669a96a 86 void downZ(){
kenken0721 0:333ed669a96a 87 if(ID == 0 || ID == 3){
kenken0721 0:333ed669a96a 88 z1 = 1;
kenken0721 0:333ed669a96a 89 z2 = 0;
kenken0721 0:333ed669a96a 90 }else{
kenken0721 0:333ed669a96a 91 z1 = 0;
kenken0721 0:333ed669a96a 92 z2 = 1;
kenken0721 0:333ed669a96a 93 }
kenken0721 0:333ed669a96a 94 }
kenken0721 0:333ed669a96a 95
kenken0721 0:333ed669a96a 96 void forwardY(){
kenken0721 0:333ed669a96a 97 if(ID == 0 || ID == 2){
kenken0721 0:333ed669a96a 98 y1 = 1;
kenken0721 0:333ed669a96a 99 y2 = 0;
kenken0721 0:333ed669a96a 100 }else{
kenken0721 0:333ed669a96a 101 y1 = 0;
kenken0721 0:333ed669a96a 102 y2 = 1;
kenken0721 0:333ed669a96a 103 }
kenken0721 0:333ed669a96a 104 }
kenken0721 0:333ed669a96a 105 void backY(){
kenken0721 0:333ed669a96a 106 if(ID == 0 || ID == 2){
kenken0721 0:333ed669a96a 107 y1 = 0;
kenken0721 0:333ed669a96a 108 y2 = 1;
kenken0721 0:333ed669a96a 109 }else{
kenken0721 0:333ed669a96a 110 y1 = 1;
kenken0721 0:333ed669a96a 111 y2 = 0;
kenken0721 0:333ed669a96a 112 }
kenken0721 0:333ed669a96a 113 }
kenken0721 0:333ed669a96a 114 void stopZ(){
kenken0721 0:333ed669a96a 115 z1 = 1;
kenken0721 0:333ed669a96a 116 z2 = 1;
kenken0721 0:333ed669a96a 117 }
kenken0721 0:333ed669a96a 118 void stopY(){
kenken0721 0:333ed669a96a 119 y1 = 1;
kenken0721 0:333ed669a96a 120 y2 = 1;
kenken0721 0:333ed669a96a 121 }
kenken0721 0:333ed669a96a 122 void Arise(){
kenken0721 0:333ed669a96a 123 if(int(B) == 0){
kenken0721 0:333ed669a96a 124 Count++;
kenken0721 0:333ed669a96a 125 }else{
kenken0721 0:333ed669a96a 126 Count--;
kenken0721 0:333ed669a96a 127 }
kenken0721 0:333ed669a96a 128
kenken0721 0:333ed669a96a 129 }
kenken0721 0:333ed669a96a 130
kenken0721 0:333ed669a96a 131 void Afall(){
kenken0721 0:333ed669a96a 132 if(int(B) == 0){
kenken0721 0:333ed669a96a 133 Count--;
kenken0721 0:333ed669a96a 134 }else{
kenken0721 0:333ed669a96a 135 Count++;
kenken0721 0:333ed669a96a 136 }
kenken0721 0:333ed669a96a 137
kenken0721 0:333ed669a96a 138 }
kenken0721 0:333ed669a96a 139
kenken0721 0:333ed669a96a 140 void Update(){
kenken0721 0:333ed669a96a 141 /*
kenken0721 0:333ed669a96a 142 if(ID == 1 || ID == 2){
kenken0721 0:333ed669a96a 143 Count = Count * -1;
kenken0721 0:333ed669a96a 144 }
kenken0721 0:333ed669a96a 145 */
kenken0721 0:333ed669a96a 146 long now = Count;
kenken0721 0:333ed669a96a 147 if(ID == 1 || ID == 2){
kenken0721 0:333ed669a96a 148 now = now * -1;
kenken0721 0:333ed669a96a 149 }
kenken0721 0:333ed669a96a 150
kenken0721 0:333ed669a96a 151 p = goal - float(now);//比例
kenken0721 0:333ed669a96a 152 i += p * interval;//積分
kenken0721 0:333ed669a96a 153 d = (p - prep)/interval;//微分
kenken0721 0:333ed669a96a 154 prep = p;
kenken0721 0:333ed669a96a 155 float output = (Kp*p + Ki*i + Kd*d);
kenken0721 0:333ed669a96a 156 if(output > 0){
kenken0721 0:333ed669a96a 157 led = 0;
kenken0721 0:333ed669a96a 158 //upZ();
kenken0721 0:333ed669a96a 159 downZ();
kenken0721 0:333ed669a96a 160 if(switch1.read() == 0){
kenken0721 0:333ed669a96a 161 stopZ();
kenken0721 0:333ed669a96a 162 output = 0;
kenken0721 0:333ed669a96a 163 }
kenken0721 0:333ed669a96a 164
kenken0721 0:333ed669a96a 165 //z1 = 0;
kenken0721 0:333ed669a96a 166 //z2 = 1;
kenken0721 0:333ed669a96a 167
kenken0721 0:333ed669a96a 168 }else if(output < 0){
kenken0721 0:333ed669a96a 169 upZ();
kenken0721 0:333ed669a96a 170
kenken0721 0:333ed669a96a 171 //downZ();
kenken0721 0:333ed669a96a 172 }else{
kenken0721 0:333ed669a96a 173 stopZ();
kenken0721 0:333ed669a96a 174 output = 0;
kenken0721 0:333ed669a96a 175 finishFlagZ = true;
kenken0721 0:333ed669a96a 176 }
kenken0721 0:333ed669a96a 177 if(output > (1 - basisPowZ)){
kenken0721 0:333ed669a96a 178 output = 1 - basisPowZ;
kenken0721 0:333ed669a96a 179 }else if(output < (basisPowZ - 1)){
kenken0721 0:333ed669a96a 180 output = basisPowZ -1;
kenken0721 0:333ed669a96a 181 }
kenken0721 0:333ed669a96a 182 if(p <= 3 && p >= -3){
kenken0721 0:333ed669a96a 183 stopZ();
kenken0721 0:333ed669a96a 184 output = 0;
kenken0721 0:333ed669a96a 185 finishFlagZ = true;
kenken0721 0:333ed669a96a 186 }
kenken0721 0:333ed669a96a 187 output = abs(output);
kenken0721 0:333ed669a96a 188 pwmZ = basisPowZ + output;
kenken0721 0:333ed669a96a 189 }
kenken0721 0:333ed669a96a 190
kenken0721 0:333ed669a96a 191 int main(){
kenken0721 0:333ed669a96a 192 led = 1;
kenken0721 0:333ed669a96a 193 Reseive.frequency(100000);
kenken0721 0:333ed669a96a 194 Reseive.attach( &inter);
kenken0721 0:333ed669a96a 195 switch1.mode(PullDown);
kenken0721 0:333ed669a96a 196 switch2.mode(PullDown);
kenken0721 0:333ed669a96a 197 switch3.mode(PullDown);
kenken0721 0:333ed669a96a 198 pwmZ.period_us(50);
kenken0721 0:333ed669a96a 199 pwmY.period_us(50);
kenken0721 0:333ed669a96a 200
kenken0721 0:333ed669a96a 201 //初期位置
kenken0721 0:333ed669a96a 202 pwmZ = basisPowZ;
kenken0721 0:333ed669a96a 203 downZ();
kenken0721 0:333ed669a96a 204 pwmY = basisPowY;
kenken0721 0:333ed669a96a 205 stopY();
kenken0721 0:333ed669a96a 206 while(1){
kenken0721 0:333ed669a96a 207 if(switch1.read() == 0){
kenken0721 0:333ed669a96a 208 stopZ();
kenken0721 0:333ed669a96a 209 break;
kenken0721 0:333ed669a96a 210 }
kenken0721 0:333ed669a96a 211 }
kenken0721 0:333ed669a96a 212 pid.attach(&Update,interval);
kenken0721 0:333ed669a96a 213 A.mode(PullUp);
kenken0721 0:333ed669a96a 214 B.mode(PullUp);
kenken0721 0:333ed669a96a 215 A.rise(&Arise);
kenken0721 0:333ed669a96a 216 A.fall(&Afall);
kenken0721 0:333ed669a96a 217 //led = 0;
kenken0721 0:333ed669a96a 218 while (true) {
kenken0721 0:333ed669a96a 219 if(finishFlagY == false){
kenken0721 0:333ed669a96a 220 if(ymode == 1){
kenken0721 0:333ed669a96a 221 if(switch2.read() == 0){
kenken0721 0:333ed669a96a 222 stopY();
kenken0721 0:333ed669a96a 223 finishFlagY = true;
kenken0721 0:333ed669a96a 224 }else{//前へ
kenken0721 0:333ed669a96a 225 forwardY();
kenken0721 0:333ed669a96a 226 }
kenken0721 0:333ed669a96a 227 }else{
kenken0721 0:333ed669a96a 228 if(switch3.read() == 0){
kenken0721 0:333ed669a96a 229 stopY();
kenken0721 0:333ed669a96a 230 finishFlagY = true;
kenken0721 0:333ed669a96a 231 }else{//後ろへ
kenken0721 0:333ed669a96a 232 backY();
kenken0721 0:333ed669a96a 233 }
kenken0721 0:333ed669a96a 234 }
kenken0721 0:333ed669a96a 235 }else{
kenken0721 0:333ed669a96a 236 stopY();
kenken0721 0:333ed669a96a 237 }
kenken0721 0:333ed669a96a 238 }
kenken0721 0:333ed669a96a 239 }