project-R
/
mbed_Enc_Mt
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
mbed_Enc_Mt
by 
taka yamanouchi
Revision 0:5b33e3942d43, committed 2018-12-27
- Comitter:
- sink
- Date:
- Thu Dec 27 01:34:33 2018 +0000
- Commit message:
- shashutunomi
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI2.lib Thu Dec 27 01:34:33 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:34:33 2018 +0000 @@ -0,0 +1,15 @@ +#define SB_ADRS 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 27 01:34:33 2018 +0000
@@ -0,0 +1,105 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "define.h" //ステータス用
+
+Ticker timer;
+Timer T;
+
+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);
+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 = 0;
+ 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_rot = 0.0;
+ float Ksp = 0.005, Ksd = 0.0015;
+ float ppr = 1.0;
+ static float pre_spd = 0.0;
+ static float pre_err = 0.0;
+ static float ref_spd = 0.0;
+ static int cmd = 0;
+ static int mode = 2;
+ static int lim_cmd = 127;
+
+ int sw_point = Button();
+
+ if(sw_point != HIGH) switch(mode){
+ case(0):
+ ref_spd = 30.0;
+ if (sw_point == RISE) mode = 1;
+ break;
+
+ case(1):
+ fet2 = ON;
+ if (sw_point == RISE) mode = 2;
+ break;
+
+ case(2):
+ ref_spd = 0.0;
+ fet2 = OFF;
+ if (sw_point == RISE) mode = 0;
+ break;
+ }
+
+ int encount2 = Enc2.getPulses();
+ int encount3 = Enc3.getPulses();
+
+ if (encount2 > encount3) encount_rot = encount2;
+ else encount_rot = encount3;
+
+ float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE;
+ float spd = (rot_sp - pre_spd)/INT_TIME/(48*4);
+
+ float spd_err = ref_spd - spd;
+ float spd_d = (spd_err - pre_err)/INT_TIME;
+ cmd += spd_err * Ksp + spd_d * Ksd;
+
+ if (cmd > lim_cmd) cmd = lim_cmd;
+ if (-cmd < -lim_cmd) cmd = -lim_cmd;
+
+ if (cmd > 0) {
+ Saber.putc(SB_ADRS);
+ Saber.putc(1);
+ Saber.putc(cmd);
+ Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
+ }
+ else {
+ Saber.putc(SB_ADRS);
+ Saber.putc(0);
+ Saber.putc(abs(cmd));
+ Saber.putc((SB_ADRS + 0 + abs(cmd)) & 0b01111111);
+ }
+
+ pre_spd = spd;
+ pre_err = spd_err;
+}
+
+int main() {
+ Saber.baud(115200);
+ pc.baud(9600);
+ timer.attach(timer_warikomi,INT_TIME);
+
+ while(1) {
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Dec 27 01:34:33 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file