taka yamanouchi
0.5
Dependencies: mbed PID QEI2 MotorDrivers Filter
Revision 0:c2f05c36e357, committed 2019-11-19
- Comitter:
- sink
- Date:
- Tue Nov 19 09:43:25 2019 +0000
- Commit message:
- feild
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Filter.lib Tue Nov 19 09:43:25 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/m204517/code/Filter/#e208acaef28d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorDrivers.lib Tue Nov 19 09:43:25 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sink/code/MotorDrivers/#9dd052490f0a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Nov 19 09:43:25 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sink/code/PID/#9dd174ee5ffc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI2.lib Tue Nov 19 09:43:25 2019 +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 Tue Nov 19 09:43:25 2019 +0000 @@ -0,0 +1,48 @@ +#define INT_TIME 0.05 + +#define RESO 48 + +#define SB_ADRS1 128 +#define SB_ADRS2 129 + +#define LIMIT_CMD 127 + +#define REF_SPD1_1 (RESO*4*(5.5))//右端 +#define REF_SPD1_2 (RESO*4*4) +#define REF_SPD2_1 (RESO*4*(5.5))//真ん中 +#define REF_SPD2_2 (RESO*4*4) +#define REF_SPD3_1 (RESO*4*6.1)//右下 +#define REF_SPD3_2 (RESO*4*(2.2)) + +#define P1 (0.12*INT_TIME) +#define D1 0.0000 + +#define P2 (0.12*INT_TIME) +#define D2 0.0000 + +#define LIMITER_ACCEL 250 +#define LIMITER_DECCEL 250 + +#define POWER_ON 1 +#define POWER_OFF 0 + +#define F1_ON 1 +#define F1_OFF (!F1_ON) +#define F2_ON 1 +#define F2_OFF (!F2_ON) +#define F3_ON 1 +#define F3_OFF (!F3_ON) +#define F4_ON 1 +#define F4_OFF (!F4_ON) + +#define CB_COLLECT 0b00000001 +#define AUTOMATION 0b00000010 + +#define LOADING 0b00000100 +#define POINTS_1 0b00001000 +#define POINTS_2 0b00010000 +#define POINTS_3 0b00100000 +#define POSITIONS 0b10000000 +#define EMERGENCY 0b01000000 + +#define EMER_COUNT 50 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Nov 19 09:43:25 2019 +0000
@@ -0,0 +1,280 @@
+#include "mbed.h"
+#include "define.h"
+#include "Sabertooth_Serial.h"
+#include "PID.h"
+#include "QEI.h"
+#include "Filter.h"
+#include "string"
+
+Ticker timer;
+Timer T;
+
+SaberSerial MD_S( 19200, p13, p14);
+RawSerial Master( p9, p10, 115200);
+RawSerial pc( USBTX, USBRX, 115200);
+
+PID pid1(INT_TIME);
+PID pid2(INT_TIME);
+
+Filter LPF1(INT_TIME);
+Filter LPF2(INT_TIME);
+
+QEI Enc1(p18,p15,NC,RESO,&T,QEI::X4_ENCODING);
+QEI Enc2(p17,p16,NC,RESO,&T,QEI::X4_ENCODING);
+//_____________________________↓機能設定
+
+DigitalIn sw1(p29);
+DigitalIn sw2(p30);
+//DigitalIn pe1(p19);
+//DigitalIn pe2(p20);
+
+DigitalOut fet1(p25);//CB回収
+DigitalOut fet2(p26);//つっかえ
+DigitalOut fet3(p27);//押出 上
+DigitalOut fet4(p28);//押出 下
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+//_____________________________↓変数宣言
+
+string Master_str = "";
+char global_mode = 0b00000000;
+int gl_enc1 = 0, gl_enc2 = 0;
+double ref_spd1 = 0, ref_spd2 = 0, load_co = 0, recv_co = 0;
+double f_spd1 = 0, f_spd2 = 0;
+int gl_cmd1 = 0, gl_cmd2 = 0;
+int gl_move = 0, gl_channel = 0;
+
+void Master_send(char mode){
+ Master.printf("%c\n",mode);
+}
+
+//______________________________↓受信割込み
+
+void Master_recv(){
+ recv_co = 1;
+ char Master_c = Master.getc();
+ static char local_mode = 0b00000000;
+
+ if(Master_c == '\n'){
+ local_mode = Master_str[0];
+ if(local_mode == EMERGENCY) {
+ global_mode = EMERGENCY;
+ }
+ else {
+ global_mode = local_mode;
+ }
+ Master_str = "";
+ }
+ else Master_str += Master_c;
+}
+//________________________________________↓速度制限・加減速制限
+
+double limit_MD(double cmd,double max_cmd){
+ if (cmd > max_cmd) cmd = max_cmd;
+ else if (cmd < -max_cmd) cmd = -max_cmd;
+ return cmd;
+}
+
+//_____________________________↓主要処理(周期割込み)
+
+void timer_warikomi(){
+ char mode = global_mode;
+ int sw1_def = sw1.read(), sw2_def = sw2.read();
+ int enc1 = Enc1.getPulses(), enc2 = Enc2.getPulses();
+ static char send_mode = 0b00000000;
+ static bool points1 = 0, points2 = 0, points3 = 0;
+ static bool positions = 0;
+ static bool pre_sw1 = 1, pre_sw2 = 1, move = 0, channel = 0;
+ static int counter = 0, point_co = 0, p1_co = 0 ,p3_co = 0, p2_co = 0;
+ static int pre_enc1 = 0, pre_enc2 = 0;
+ static double cmd1 = 0, cmd2 = 0, up_cmd = 0;
+
+
+ if(sw1_def == 0 && pre_sw1 == 1) move = !move;
+ if(sw2_def == 0 && pre_sw2 == 1) channel = !channel;
+
+ pre_sw1 = sw1_def;
+ pre_sw2 = sw2_def;
+
+ if(!recv_co);
+ if(mode == EMERGENCY){
+ f_spd1 = 0;
+ f_spd2 = 0;
+ up_cmd = 0;
+ if(counter >= EMER_COUNT){
+ led1 = !led1;
+ led2 = !led2;
+ led3 = !led3;
+ led4 = !led4;
+ counter = 0;
+ }else counter++;
+ }else if(mode & AUTOMATION){
+ up_cmd = 0;
+ led3 = 1;
+ led4 = 1;
+ points1 = (mode & POINTS_1);//20
+ points2 = (mode & POINTS_2);//40
+ points3 = (mode & POINTS_3);//10
+ positions = (mode & POSITIONS);
+
+ if(!points1 && !points2 && !points3){
+ f_spd1 = 0;
+ f_spd2 = 0;
+ }
+ if (points2){
+ led4 = 1;
+ f_spd1 = REF_SPD2_1;
+ f_spd2 = REF_SPD2_2;
+ send_mode |= POINTS_2;
+ if(positions && (enc1 - pre_enc1) > (REF_SPD2_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
+ if(p1_co < 3){
+ point_co++;
+ if(point_co >= 20){
+ fet3 = F3_ON;
+ p1_co++;
+ point_co = 0;
+ }else fet3 = F3_OFF;
+ }else{
+ send_mode |= POSITIONS;
+ fet3 = F3_OFF;
+ }
+ }else{
+ send_mode &= ~POINTS_1;
+ send_mode &= ~POSITIONS;
+ }
+ }if(points1){
+ led4 = 1;
+ f_spd1 = REF_SPD1_1;
+ f_spd2 = REF_SPD1_2;
+ send_mode |= POINTS_1;
+ if(positions && (enc1 - pre_enc1) > (REF_SPD1_1 - 30) && (enc1 - pre_enc1) < (REF_SPD1_1 + 30)){
+ fet3 = F3_ON;
+ if(p2_co > 8) send_mode |= POSITIONS;
+ else p2_co++;
+ }else{
+ fet3 = F3_OFF;
+ send_mode &= ~POINTS_3;
+ send_mode &= ~POSITIONS;
+ }
+ }if(points3){
+ led4 = 1;
+ fet2 = F2_OFF;
+ fet3 = F3_OFF;
+ f_spd1 = REF_SPD3_1;
+ f_spd2 = REF_SPD3_2;
+ send_mode |= POINTS_3;
+ if(positions && (enc1 - pre_enc1) > (REF_SPD3_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
+ if(p3_co < 6){
+ if (point_co >= 20){
+ fet4 = F4_ON;
+ point_co = 0;
+ p3_co++;
+ }else {
+ fet4 = F4_OFF;
+ send_mode &= ~POINTS_2;
+ send_mode &= ~POSITIONS;
+ }
+ }else{
+ if(p3_co > 15){
+ fet4 = F4_OFF;
+ send_mode |= POSITIONS;
+ f_spd1 = 0;
+ f_spd2 = 0;
+ }else p3_co++;
+ }
+ }
+ }else if(positions){
+ led4 = 1;
+ send_mode |= POSITIONS;
+ }else if(!positions){
+ send_mode &= ~POSITIONS;
+ }else;
+
+ Master_send(send_mode);
+ }else{
+ /*led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;*/
+ if(mode & CB_COLLECT){
+ up_cmd = 50;
+ fet1 = F1_ON;
+ }else{
+ fet1 = F1_OFF;
+ up_cmd = 0;
+ }
+ if(move){
+ led1 = 1;
+ led2 = 0;
+ f_spd1 = REF_SPD3_1;
+ f_spd2 = REF_SPD3_2;
+ if(channel) fet4 = F4_ON;
+ else fet4 = F4_OFF;
+ }else{
+ led1 = 0;
+ led2 = 1;
+ f_spd1 = 0;
+ f_spd2 = 0;
+ if(channel) fet4 = F4_ON;
+ else fet4 = F4_OFF;
+ }
+ }
+ gl_enc1 = enc1 - pre_enc1;
+ gl_enc2 = enc2 - pre_enc2;
+
+ ref_spd1 = LPF1.LowPassFilter(f_spd1);
+ ref_spd2 = LPF2.LowPassFilter(f_spd2);
+
+ cmd1 += pid1.con(ref_spd1 - gl_enc1);
+ cmd1 = limit_MD( cmd1, 120);
+ if(f_spd1 == 0 && abs(cmd1) < 20) cmd1 = 0;
+ if(cmd1 >= 0) MD_S.Serial(SB_ADRS1, 1, (int)(abs(cmd1)));
+ else MD_S.Serial(SB_ADRS1, 0, (int)(abs(cmd1)));
+
+ cmd2 += pid2.con(ref_spd2 - gl_enc2);
+ cmd2 = limit_MD( cmd2, 120);
+ if(f_spd2 == 0 && abs(cmd2) < 20) cmd2 = 0;
+ if(cmd2 >= 0) MD_S.Serial(SB_ADRS1, 5, (int)(abs(cmd2)));
+ else MD_S.Serial(SB_ADRS1, 6, (int)(abs(cmd2)));
+
+ pre_enc1 = enc1;
+ pre_enc2 = enc2;
+
+ gl_cmd1 = cmd1;
+ gl_cmd2 = cmd2;
+ gl_move = move;
+ gl_channel = channel;
+
+ MD_S.Serial(SB_ADRS2, 5, up_cmd);
+}
+//__________________________________________
+
+int main() {
+ fet1 = F1_OFF;
+ fet2 = F2_OFF;
+ fet3 = F3_OFF;
+ fet4 = F4_ON;
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+
+ pid1.set(P1,NU,D1);
+ pid2.set(P2,NU,D1);
+
+ LPF1.setLowPassPara(2.0, 0);
+ LPF2.setLowPassPara(2.0, 0);
+
+ timer.attach(&timer_warikomi,INT_TIME);
+ Master.attach(&Master_recv ,RawSerial::RxIrq);
+ while(true) {
+ /*static int buff1 = 0, buff2 = 0;
+ buff1 = Enc1.getPulses();
+ buff2 = Enc2.getPulses();*/
+ pc.printf("ERR1:%d ERR2:%d CMD1:%d CMD2:%d MODE:%d CAHN:%d\n", gl_enc1, gl_enc2, gl_cmd1, gl_cmd2, int(global_mode), gl_channel);
+ wait(0.1);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Nov 19 09:43:25 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file