project-R
toutekikikou_Program
Revision 0:56c3d27ab161, committed 2018-12-27
- Comitter:
- sink
- Date:
- Thu Dec 27 01:51:51 2018 +0000
- Commit message:
- touteki_zissouyoteiban
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI2.lib Thu Dec 27 01:51:51 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/kikoaac/code/QEI2/#49fa8827718d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/define.h Thu Dec 27 01:51:51 2018 +0000 @@ -0,0 +1,17 @@ +#define SB_ADRS_A 129 +#define SB_ADRS_B 132 + +#define INT_TIME 0.02 +#define RESOLUTION 48 + +#define ON 1 +#define OFF 0 + +#define HIGH 0 +#define FALL 1 +#define RISE 2 +#define LOW 3 + +#define GEAR_RATE 1.0 +#define PULL_RATE 1.0 +#define MULTIPLU 4.0 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 27 01:51:51 2018 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "define.h" //ステータス用
+
+Ticker timer;
+Timer T;
+
+QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+
+DigitalIn sw1(p26);
+DigitalIn sw2(p25);
+DigitalIn limit1(p15);
+DigitalIn limit2(p16);
+DigitalIn sensor1(p18);
+DigitalIn sensor2(p17);
+
+DigitalOut fet1(p21);
+DigitalOut fet2(p22);
+
+Serial Saber(p13,p14);
+Serial pc(USBTX,USBRX);
+
+int Button() {
+ int button_in = sw1.read();
+ static int pre_button = 1;
+ static int sw_state = LOW;
+
+ if(button_in && pre_button)sw_state = HIGH;
+ if(!button_in && !pre_button)sw_state = LOW;
+ if(button_in && !pre_button)sw_state = FALL;
+ if(!button_in && pre_button)sw_state = RISE;
+
+ pre_button = button_in;
+
+ return sw_state;
+}
+
+void timer_warikomi()
+{
+ float encount_ang = 0.0;
+ float encount_rot = 0.0;
+ float Ksp = 0.005, Ksd = 0.0015; //速度係数
+ float Kp=5.0, Ki=0.01, Kd=0.1; //角度係数
+ //float Kp_V=1, Kd_V=0; //角速度係数
+ float ppr = 1.0;
+ static float pre_angle = 0.0;
+ static float pre_angleE = 0.0;
+ static float pre_spd = 0.0;
+ static float pre_spdE = 0.0;
+ static float ref_angle = 0.0;
+ static float ref_spd = 0.0;
+ static float angle_I = 0.0;
+ static int cmd_spd = 0;
+ static int cmd_ang = 0;
+ static int mode = 0;
+ static int lim_cmdA = 20;
+ static int lim_cmdS = 127;
+ static int pre_encount = 0;
+
+ int sw_point = Button();
+
+ int encount2 = Enc2.getPulses();
+ int encount3 = Enc3.getPulses();
+
+ encount_ang = Enc1.getPulses()- pre_encount;
+ if (encount2 > encount3) encount_rot = encount2;
+ else encount_rot = encount3;
+
+ float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0;
+ float ang_spd =(angle - pre_angle)/INT_TIME;
+
+ float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE;
+ float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU);
+
+ float angle_P = (ref_angle - angle);
+ float angle_D=(angle_P - pre_angleE)/INT_TIME;
+ angle_I += (angle_P + pre_angleE)*INT_TIME/2.0;
+
+ cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki);
+
+ float spd_e = ref_spd - spd;
+ float spd_D = (spd_e - pre_spdE)/INT_TIME;
+ cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd);
+
+ if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA;
+ if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA;
+
+ if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS;
+ if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS;
+
+ if (sw_point != HIGH) switch (mode) {
+
+ case 0:
+ ref_angle = 0;
+ ref_spd = 0.0;
+ fet2 = OFF;
+ if (sw_point == RISE) mode = 1;
+ break;
+
+ case 1:
+ cmd_ang = 20;
+ if (limit1.read()) {
+ cmd_ang = 0;
+ pre_encount = Enc1.getPulses();
+ if (sw_point == RISE) mode = 2;
+ }
+ break;
+
+ case 2:
+ ref_angle = -125;
+ if (sw_point == RISE) mode = 3;
+ break;
+
+ case 3:
+ if (!sensor1 && !sensor2) {
+ fet1 = ON;
+ if (sw_point == RISE) mode = 4;
+ }
+ break;
+
+ case 4:
+ ref_angle = 0;
+ //if(99<=angle||angle<=101){
+ if (sw_point == RISE) {
+ cmd_ang = 0;
+ pre_encount = Enc1.getPulses(); //スイッチを離したところを初期値に
+ mode = 5; //して無理くり止めてます
+ }
+ break;
+
+ case 5:
+ fet1 = OFF;
+ ref_spd = 30.0;
+ if (sw_point == RISE) mode = 6;
+ break;
+
+ case 6:
+ fet2 = ON;
+ if (sw_point == RISE) mode = 0;
+ break;
+ }
+
+ if (!sw2.read()) {
+ cmd_spd = 0;
+ cmd_ang = 0;
+ }
+
+ if (cmd_ang >= 0) {
+ Saber.putc(SB_ADRS_A);
+ Saber.putc(1);
+ Saber.putc(cmd_ang);
+ Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111);
+ }
+ else {
+ Saber.putc(SB_ADRS_A);
+ Saber.putc(0);
+ Saber.putc(abs(cmd_ang));
+ Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111);
+ }
+
+ if (cmd_spd >= 0) {
+ Saber.putc(SB_ADRS_B);
+ Saber.putc(1);
+ Saber.putc(cmd_spd);
+ Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111);
+ }
+ else {
+ Saber.putc(SB_ADRS_B);
+ Saber.putc(0);
+ Saber.putc(abs(cmd_spd));
+ Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111);
+ }
+
+ pre_spd = spd;
+ pre_spdE = spd_e;
+ pre_angle = angle;
+ pre_angleE = angle_P;
+}
+
+int main() {
+ Saber.baud(115200);
+ pc.baud(9600);
+ timer.attach(timer_warikomi,INT_TIME);
+
+ while(1) {
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Dec 27 01:51:51 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file