project-R / Mbed 2 deprecated mbed_touteki_MR1

toutekikikou_Program

Dependencies:   mbed QEI2

main.cpp@0:56c3d27ab161, 2018-12-27 (annotated)

Committer:
sink
Date:
Thu Dec 27 01:51:51 2018 +0000
Revision:
0:56c3d27ab161
touteki_zissouyoteiban

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:56c3d27ab161 1 #include "mbed.h"
sink 0:56c3d27ab161 2 #include "QEI.h"
sink 0:56c3d27ab161 3 #include "define.h" //ステータス用
sink 0:56c3d27ab161 4
sink 0:56c3d27ab161 5 Ticker timer;
sink 0:56c3d27ab161 6 Timer T;
sink 0:56c3d27ab161 7
sink 0:56c3d27ab161 8 QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 0:56c3d27ab161 9 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 0:56c3d27ab161 10 QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 0:56c3d27ab161 11
sink 0:56c3d27ab161 12 DigitalIn sw1(p26);
sink 0:56c3d27ab161 13 DigitalIn sw2(p25);
sink 0:56c3d27ab161 14 DigitalIn limit1(p15);
sink 0:56c3d27ab161 15 DigitalIn limit2(p16);
sink 0:56c3d27ab161 16 DigitalIn sensor1(p18);
sink 0:56c3d27ab161 17 DigitalIn sensor2(p17);
sink 0:56c3d27ab161 18
sink 0:56c3d27ab161 19 DigitalOut fet1(p21);
sink 0:56c3d27ab161 20 DigitalOut fet2(p22);
sink 0:56c3d27ab161 21
sink 0:56c3d27ab161 22 Serial Saber(p13,p14);
sink 0:56c3d27ab161 23 Serial pc(USBTX,USBRX);
sink 0:56c3d27ab161 24
sink 0:56c3d27ab161 25 int Button() {
sink 0:56c3d27ab161 26 int button_in = sw1.read();
sink 0:56c3d27ab161 27 static int pre_button = 1;
sink 0:56c3d27ab161 28 static int sw_state = LOW;
sink 0:56c3d27ab161 29
sink 0:56c3d27ab161 30 if(button_in && pre_button)sw_state = HIGH;
sink 0:56c3d27ab161 31 if(!button_in && !pre_button)sw_state = LOW;
sink 0:56c3d27ab161 32 if(button_in && !pre_button)sw_state = FALL;
sink 0:56c3d27ab161 33 if(!button_in && pre_button)sw_state = RISE;
sink 0:56c3d27ab161 34
sink 0:56c3d27ab161 35 pre_button = button_in;
sink 0:56c3d27ab161 36
sink 0:56c3d27ab161 37 return sw_state;
sink 0:56c3d27ab161 38 }
sink 0:56c3d27ab161 39
sink 0:56c3d27ab161 40 void timer_warikomi()
sink 0:56c3d27ab161 41 {
sink 0:56c3d27ab161 42 float encount_ang = 0.0;
sink 0:56c3d27ab161 43 float encount_rot = 0.0;
sink 0:56c3d27ab161 44 float Ksp = 0.005, Ksd = 0.0015; //速度係数
sink 0:56c3d27ab161 45 float Kp=5.0, Ki=0.01, Kd=0.1; //角度係数
sink 0:56c3d27ab161 46 //float Kp_V=1, Kd_V=0; //角速度係数
sink 0:56c3d27ab161 47 float ppr = 1.0;
sink 0:56c3d27ab161 48 static float pre_angle = 0.0;
sink 0:56c3d27ab161 49 static float pre_angleE = 0.0;
sink 0:56c3d27ab161 50 static float pre_spd = 0.0;
sink 0:56c3d27ab161 51 static float pre_spdE = 0.0;
sink 0:56c3d27ab161 52 static float ref_angle = 0.0;
sink 0:56c3d27ab161 53 static float ref_spd = 0.0;
sink 0:56c3d27ab161 54 static float angle_I = 0.0;
sink 0:56c3d27ab161 55 static int cmd_spd = 0;
sink 0:56c3d27ab161 56 static int cmd_ang = 0;
sink 0:56c3d27ab161 57 static int mode = 0;
sink 0:56c3d27ab161 58 static int lim_cmdA = 20;
sink 0:56c3d27ab161 59 static int lim_cmdS = 127;
sink 0:56c3d27ab161 60 static int pre_encount = 0;
sink 0:56c3d27ab161 61
sink 0:56c3d27ab161 62 int sw_point = Button();
sink 0:56c3d27ab161 63
sink 0:56c3d27ab161 64 int encount2 = Enc2.getPulses();
sink 0:56c3d27ab161 65 int encount3 = Enc3.getPulses();
sink 0:56c3d27ab161 66
sink 0:56c3d27ab161 67 encount_ang = Enc1.getPulses()- pre_encount;
sink 0:56c3d27ab161 68 if (encount2 > encount3) encount_rot = encount2;
sink 0:56c3d27ab161 69 else encount_rot = encount3;
sink 0:56c3d27ab161 70
sink 0:56c3d27ab161 71 float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0;
sink 0:56c3d27ab161 72 float ang_spd =(angle - pre_angle)/INT_TIME;
sink 0:56c3d27ab161 73
sink 0:56c3d27ab161 74 float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE;
sink 0:56c3d27ab161 75 float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU);
sink 0:56c3d27ab161 76
sink 0:56c3d27ab161 77 float angle_P = (ref_angle - angle);
sink 0:56c3d27ab161 78 float angle_D=(angle_P - pre_angleE)/INT_TIME;
sink 0:56c3d27ab161 79 angle_I += (angle_P + pre_angleE)*INT_TIME/2.0;
sink 0:56c3d27ab161 80
sink 0:56c3d27ab161 81 cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki);
sink 0:56c3d27ab161 82
sink 0:56c3d27ab161 83 float spd_e = ref_spd - spd;
sink 0:56c3d27ab161 84 float spd_D = (spd_e - pre_spdE)/INT_TIME;
sink 0:56c3d27ab161 85 cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd);
sink 0:56c3d27ab161 86
sink 0:56c3d27ab161 87 if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA;
sink 0:56c3d27ab161 88 if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA;
sink 0:56c3d27ab161 89
sink 0:56c3d27ab161 90 if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS;
sink 0:56c3d27ab161 91 if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS;
sink 0:56c3d27ab161 92
sink 0:56c3d27ab161 93 if (sw_point != HIGH) switch (mode) {
sink 0:56c3d27ab161 94
sink 0:56c3d27ab161 95 case 0:
sink 0:56c3d27ab161 96 ref_angle = 0;
sink 0:56c3d27ab161 97 ref_spd = 0.0;
sink 0:56c3d27ab161 98 fet2 = OFF;
sink 0:56c3d27ab161 99 if (sw_point == RISE) mode = 1;
sink 0:56c3d27ab161 100 break;
sink 0:56c3d27ab161 101
sink 0:56c3d27ab161 102 case 1:
sink 0:56c3d27ab161 103 cmd_ang = 20;
sink 0:56c3d27ab161 104 if (limit1.read()) {
sink 0:56c3d27ab161 105 cmd_ang = 0;
sink 0:56c3d27ab161 106 pre_encount = Enc1.getPulses();
sink 0:56c3d27ab161 107 if (sw_point == RISE) mode = 2;
sink 0:56c3d27ab161 108 }
sink 0:56c3d27ab161 109 break;
sink 0:56c3d27ab161 110
sink 0:56c3d27ab161 111 case 2:
sink 0:56c3d27ab161 112 ref_angle = -125;
sink 0:56c3d27ab161 113 if (sw_point == RISE) mode = 3;
sink 0:56c3d27ab161 114 break;
sink 0:56c3d27ab161 115
sink 0:56c3d27ab161 116 case 3:
sink 0:56c3d27ab161 117 if (!sensor1 && !sensor2) {
sink 0:56c3d27ab161 118 fet1 = ON;
sink 0:56c3d27ab161 119 if (sw_point == RISE) mode = 4;
sink 0:56c3d27ab161 120 }
sink 0:56c3d27ab161 121 break;
sink 0:56c3d27ab161 122
sink 0:56c3d27ab161 123 case 4:
sink 0:56c3d27ab161 124 ref_angle = 0;
sink 0:56c3d27ab161 125 //if(99<=angle||angle<=101){
sink 0:56c3d27ab161 126 if (sw_point == RISE) {
sink 0:56c3d27ab161 127 cmd_ang = 0;
sink 0:56c3d27ab161 128 pre_encount = Enc1.getPulses(); //スイッチを離したところを初期値に
sink 0:56c3d27ab161 129 mode = 5; //して無理くり止めてます
sink 0:56c3d27ab161 130 }
sink 0:56c3d27ab161 131 break;
sink 0:56c3d27ab161 132
sink 0:56c3d27ab161 133 case 5:
sink 0:56c3d27ab161 134 fet1 = OFF;
sink 0:56c3d27ab161 135 ref_spd = 30.0;
sink 0:56c3d27ab161 136 if (sw_point == RISE) mode = 6;
sink 0:56c3d27ab161 137 break;
sink 0:56c3d27ab161 138
sink 0:56c3d27ab161 139 case 6:
sink 0:56c3d27ab161 140 fet2 = ON;
sink 0:56c3d27ab161 141 if (sw_point == RISE) mode = 0;
sink 0:56c3d27ab161 142 break;
sink 0:56c3d27ab161 143 }
sink 0:56c3d27ab161 144
sink 0:56c3d27ab161 145 if (!sw2.read()) {
sink 0:56c3d27ab161 146 cmd_spd = 0;
sink 0:56c3d27ab161 147 cmd_ang = 0;
sink 0:56c3d27ab161 148 }
sink 0:56c3d27ab161 149
sink 0:56c3d27ab161 150 if (cmd_ang >= 0) {
sink 0:56c3d27ab161 151 Saber.putc(SB_ADRS_A);
sink 0:56c3d27ab161 152 Saber.putc(1);
sink 0:56c3d27ab161 153 Saber.putc(cmd_ang);
sink 0:56c3d27ab161 154 Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111);
sink 0:56c3d27ab161 155 }
sink 0:56c3d27ab161 156 else {
sink 0:56c3d27ab161 157 Saber.putc(SB_ADRS_A);
sink 0:56c3d27ab161 158 Saber.putc(0);
sink 0:56c3d27ab161 159 Saber.putc(abs(cmd_ang));
sink 0:56c3d27ab161 160 Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111);
sink 0:56c3d27ab161 161 }
sink 0:56c3d27ab161 162
sink 0:56c3d27ab161 163 if (cmd_spd >= 0) {
sink 0:56c3d27ab161 164 Saber.putc(SB_ADRS_B);
sink 0:56c3d27ab161 165 Saber.putc(1);
sink 0:56c3d27ab161 166 Saber.putc(cmd_spd);
sink 0:56c3d27ab161 167 Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111);
sink 0:56c3d27ab161 168 }
sink 0:56c3d27ab161 169 else {
sink 0:56c3d27ab161 170 Saber.putc(SB_ADRS_B);
sink 0:56c3d27ab161 171 Saber.putc(0);
sink 0:56c3d27ab161 172 Saber.putc(abs(cmd_spd));
sink 0:56c3d27ab161 173 Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111);
sink 0:56c3d27ab161 174 }
sink 0:56c3d27ab161 175
sink 0:56c3d27ab161 176 pre_spd = spd;
sink 0:56c3d27ab161 177 pre_spdE = spd_e;
sink 0:56c3d27ab161 178 pre_angle = angle;
sink 0:56c3d27ab161 179 pre_angleE = angle_P;
sink 0:56c3d27ab161 180 }
sink 0:56c3d27ab161 181
sink 0:56c3d27ab161 182 int main() {
sink 0:56c3d27ab161 183 Saber.baud(115200);
sink 0:56c3d27ab161 184 pc.baud(9600);
sink 0:56c3d27ab161 185 timer.attach(timer_warikomi,INT_TIME);
sink 0:56c3d27ab161 186
sink 0:56c3d27ab161 187 while(1) {
sink 0:56c3d27ab161 188 }
sink 0:56c3d27ab161 189 }