Diff: main.cpp

Revision:

0:a1f453dd2bb5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 12 05:50:18 2022 +0000
@@ -0,0 +1,492 @@
+#include "mbed.h"
+#include "IncEncoder.h"
+#include <math.h>
+
+#define PI 3.14159265358979
+
+#define BELT_RPP 512
+#define ANG_RPP 512
+#define LIFT_RPP 256
+
+#define BELT_PER 1050
+#define BELT_Rratio 5
+
+#define LIFT_PCD 54 
+
+#define BELT_UC 30 //モータ出力オフセット
+#define ANG_UC 25
+#define LIFT_UC 36
+
+#define VELO_dt 0.010
+#define BELT_dt 0.010
+#define ANG_dt 0.010
+#define LIFT_dt 0.010
+
+#define VELO_Ke 1
+#define VELO_Kp 0.005
+#define VELO_Ki 0.000012
+#define VELO_Kd 0.0000
+
+#define BELT_Ke 1
+#define BELT_Kp 1.2
+#define BELT_Ki 0.005
+#define BELT_Kd 0.03
+
+#define ANG_Ke 4
+#define ANG_Kp 1.2
+#define ANG_Ki 0.005
+#define ANG_Kd 0.03
+
+#define LIFT_Ke 2
+#define LIFT_Kp 1.8
+#define LIFT_Ki 0.013
+#define LIFT_Kd 0.03
+
+#define ARM_SERVO_OFFSET 7500
+#define ARM_SERVO_VPA 29.6296296
+
+//モータードライバのアドレス
+#define beltR_MOTOR 0x02
+#define angR_MOTOR 0x04
+
+//モータードライバへの命令
+#define STOP 0
+#define CW 1
+#define CCW -1
+#define bSTOP 10
+#define bCW 11
+#define bCCW -11
+
+#define sCW 21
+#define sCCW -21
+#define sSTOP 20
+#define shCW 31
+#define shCCW -31
+
+#define lCW 51
+#define lCCW -51
+#define lSTOP 50
+#define lsCW 61
+#define lsCCW -61
+#define lsSTOP 60
+
+#define rON 41
+#define rOFF -41
+
+//通信初期化
+Serial mecha_ins(USBTX,USBRX,115200);
+I2C md_i2c_tx(PD_13,PD_12);
+
+//GPIO初期化
+InterruptIn beltR_i_(PC_1);
+
+DigitalIn angR_zero(PE_15);
+
+//QEI初期化
+IncEncoder beltR_RE(PC_2,PC_3,IncEncoder::x2_Resolution);
+IncEncoder angR_RE(PF_9,PC_0,IncEncoder::x2_Resolution);
+
+//プロトタイプ関数宣言
+void ins_recieve();
+void BELTR_PID();
+void ANGR_PID();
+void belt_reset();
+void beltR_reset();
+void md_setoutput(unsigned char address, int rotate, int duty);
+
+volatile char led_command = 0xFF;
+
+volatile char mecha_prep = 0;
+volatile int triggerR_flag = 0, triggerL_flag = 0;
+
+volatile int lift_pid_flag_R = 1, lift_pid_flag_L = 1;
+
+volatile int kick_R = 0, kick_L = 0;
+volatile int pid_off_R = 0, pid_off_L = 0;
+
+volatile int beltR_delaycnt = 0, beltL_delaycnt = 0;
+volatile int beltR_delayflag = 1, beltL_delayflag = 1;
+
+volatile int delayR_cnt = 0, delayL_cnt = 0;
+volatile int delayR_flag = 1, delayL_flag = 1;
+
+volatile int liftR_reset = 0, liftL_reset = 0;
+volatile int stateR_num = 0, stateL_num = 0;
+volatile int cntR = 0, cntL = 0;
+
+volatile int BELTR_flag = 0, BELTL_flag = 0;
+
+volatile int beltR_POWER = 0;
+volatile int beltL_POWER = 0;
+volatile int beltR_TRIGGER = 0;
+volatile int beltL_TRIGGER = 0;
+
+volatile int beltR_UMAX = 100;
+volatile int beltL_UMAX = 100;
+
+volatile int beltR_MOTOR_Rdir;
+volatile int beltL_MOTOR_Rdir;
+volatile double beltR_DES = 20;
+volatile double beltL_DES = 20;
+volatile double beltR_STATE = 0;
+volatile double beltL_STATE = 0;
+volatile double beltR_U;
+volatile double beltL_U;
+volatile double beltR_ERROR[2] = {};
+volatile double beltL_ERROR[2] = {};
+volatile double beltR_INTEGRAL = 0;
+volatile double beltL_INTEGRAL = 0;
+
+volatile int angR_MOTOR_Rdir;
+volatile int angL_MOTOR_Rdir;
+volatile double ANGR_DES = 0;
+volatile double ANGL_DES = 0;
+volatile double ANGR_STATE = 0;
+volatile double ANGL_STATE = 0;
+volatile double ANGR_U;
+volatile double ANGL_U;
+volatile double ANGR_ERROR[2] = {};
+volatile double ANGL_ERROR[2] = {};
+volatile double ANGR_INTEGRAL = 0;
+volatile double ANGL_INTEGRAL = 0;
+
+volatile int liftR_MOTOR_Rdir;
+volatile int liftL_MOTOR_Rdir;
+volatile double liftR_DES = 0;
+volatile double liftL_DES = 0;
+volatile double liftR_STATE = 0;
+volatile double liftL_STATE = 0;
+volatile double liftR_U;
+volatile double liftL_U;
+volatile double liftR_ERROR[2] = {};
+volatile double liftL_ERROR[2] = {};
+volatile double liftR_INTEGRAL = 0;
+volatile double liftL_INTEGRAL = 0;
+
+//main
+int main(){
+    md_i2c_tx.frequency(400000); //MD用I2C
+    
+    beltR_RE.reset();
+    angR_RE.reset();
+    
+    angR_zero.mode(PullUp);
+    
+    beltR_i_.mode(PullDown);
+    
+    beltR_i_.rise(&beltR_reset);
+    
+    wait_ms(300);
+    
+    while(!angR_zero){
+        if(!angR_zero){
+            md_setoutput(angR_MOTOR,bCCW,50);
+        }else{
+            md_setoutput(angR_MOTOR,bSTOP,0);
+        }
+    }
+    
+    angR_RE.reset();
+    
+    belt_reset();
+    
+    while(mecha_ins.readable()){
+        char dummy = mecha_ins.getc();
+    }
+    
+    mecha_ins.attach(&ins_recieve, Serial::RxIrq);
+    
+    while(1){
+        if(1){
+            //追従処理呼び出し なぜかTickerが新マザーで正常動作しないため\(^o^)/
+            if(!beltR_TRIGGER){
+                BELTR_PID();
+            }
+            
+            ANGR_PID();
+            
+            //発射処理
+            if(beltR_TRIGGER == 1){
+                
+                beltR_TRIGGER++;
+            }
+            
+            if(beltR_TRIGGER == 2){
+                if(pid_off_R){
+                    md_setoutput(beltR_MOTOR,CW,beltR_POWER);
+                }else{
+                    md_setoutput(beltR_MOTOR,sCW,beltR_POWER);
+                }
+                
+                triggerR_flag = 0; 
+            }
+            
+            if(kick_R){
+                if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 2.0 && beltR_TRIGGER == 2){
+                    pid_off_R = 0;
+                    beltR_delaycnt = 50;
+                    beltR_delayflag = 0;
+                    beltR_TRIGGER++;
+                }
+            }else{
+                if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 0.5 && beltR_TRIGGER == 2){
+                    pid_off_R = 0;
+                    beltR_delaycnt = 50;
+                    beltR_delayflag = 0;
+                    beltR_TRIGGER++;
+                }
+            }
+            
+            if(beltR_TRIGGER == 3){
+                kick_R = 0;
+                md_setoutput(beltR_MOTOR,bSTOP,0);
+            }
+            
+            if(beltR_delayflag && beltR_TRIGGER == 3){
+                beltR_TRIGGER++;
+            }
+            
+            if(beltR_TRIGGER == 4){
+                md_setoutput(beltR_MOTOR,bCW,70);
+            }
+            
+            if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 5 && beltR_TRIGGER == 4){
+                BELTR_flag = 1;
+                beltR_TRIGGER++;
+            }
+            
+            if(beltR_TRIGGER == 5){
+                md_setoutput(beltR_MOTOR,bCCW,70);
+            }
+            
+            if(!BELTR_flag && beltR_TRIGGER == 5){
+                md_setoutput(beltR_MOTOR,bSTOP,0);
+                beltR_INTEGRAL = 0;
+                beltR_TRIGGER = 0;
+            }
+        }
+        
+        mecha_prep = 0x00;
+        
+        if(triggerR_flag){
+            mecha_prep = 0x80;
+        }
+        if(triggerL_flag){
+            mecha_prep = 0x40;
+        }
+        if(beltR_TRIGGER){
+            mecha_prep = 0x20;
+        }
+        if(beltL_TRIGGER){
+            mecha_prep = 0x10;
+        }
+        if(stateR_num){
+            mecha_prep = 0x08;
+        }
+        if(stateL_num){
+            mecha_prep = 0x04;
+        }
+        if(liftR_reset || liftL_reset){
+            mecha_prep = 0x02;
+        }
+        
+        mecha_ins.putc(mecha_prep);
+        
+        wait_ms(10);
+    }
+}
+
+//右発射角追従制御
+void ANGR_PID(){
+    ANGR_STATE = 360 * -angR_RE.GetIncPulses()/(ANG_RPP*2);
+    
+    ANGR_ERROR[0] = ANGR_ERROR[1];
+    ANGR_ERROR[1] = (double)ANG_Ke * (ANGR_STATE - ANGR_DES);
+    ANGR_INTEGRAL += ANGR_ERROR[1]*ANG_dt;
+    
+    ANGR_U = ANG_Kp * ANGR_ERROR[1] + ANG_Ki * ANGR_INTEGRAL + ANG_Kd * (ANGR_ERROR[1] - ANGR_ERROR[0])/ANG_dt;
+    
+    if(ANGR_U >= 0){
+        angR_MOTOR_Rdir = bCCW;
+    }else{
+        angR_MOTOR_Rdir = bCW;
+        ANGR_U *= -1;
+    }
+    
+    if(ANGR_U >= 100){
+        ANGR_U = 100;
+    }
+    
+    if(angR_zero && angR_MOTOR_Rdir == bCCW){
+        md_setoutput(angR_MOTOR,bSTOP,0);
+    }else{
+        md_setoutput(angR_MOTOR,angR_MOTOR_Rdir,ANGR_U+ANG_UC);
+    }
+}
+
+//右ベルト位置追従制御
+void BELTR_PID(){
+    beltR_STATE = BELT_PER * beltR_RE.GetIncPulses()/(BELT_RPP*2*BELT_Rratio);
+    
+    beltR_ERROR[0] = beltR_ERROR[1];
+    beltR_ERROR[1] = (double)BELT_Ke * (beltR_STATE - beltR_DES);
+    beltR_INTEGRAL += beltR_ERROR[1]*BELT_dt;
+    
+    beltR_U = BELT_Kp * beltR_ERROR[1] + BELT_Ki * beltR_INTEGRAL + BELT_Kd * (beltR_ERROR[1] - beltR_ERROR[0])/BELT_dt;
+    
+    if(beltR_U >= 0){
+        beltR_MOTOR_Rdir = bCCW;
+    }else{
+        beltR_MOTOR_Rdir = bCW;
+        beltR_U *= -1;
+    }
+    
+    if(beltR_U >= beltR_UMAX){
+        beltR_U = beltR_UMAX;
+    }
+    
+    md_setoutput(beltR_MOTOR,beltR_MOTOR_Rdir,beltR_U+BELT_UC);
+}
+
+void belt_reset(){
+    int beltR_flag = 1;
+    
+    while(beltR_flag){
+        if(360 * beltR_RE.GetIncPulses()/(BELT_RPP*2) < 180){
+            md_setoutput(beltR_MOTOR,bCW,70);
+        }else{
+            md_setoutput(beltR_MOTOR,bSTOP,0);
+            beltR_flag = 0;
+        }
+    }
+    
+    BELTR_flag = 1;
+    
+    while(BELTR_flag){
+        if(BELTR_flag){
+            md_setoutput(beltR_MOTOR,bCCW,70);
+        }else{
+            md_setoutput(beltR_MOTOR,bSTOP,0);
+        }
+    }
+    
+    wait_ms(10);
+}
+
+//命令受信処理
+void ins_recieve(){
+    char ins_byte = mecha_ins.getc();
+    
+    switch(ins_byte){
+        case 0x0F : 
+            cntR = mecha_ins.getc();
+            cntL = mecha_ins.getc();
+            liftR_reset = 1;
+            liftL_reset = 1;
+            break;
+        case 0xA1 : 
+            beltR_POWER = mecha_ins.getc();
+            beltR_TRIGGER = 1;
+            break;
+        case 0xA2 : 
+            cntR = mecha_ins.getc();
+            stateR_num = 1;
+            break;
+        case 0xA3 : 
+            ANGR_DES = mecha_ins.getc();
+            break;
+        case 0xB1 : 
+             beltL_POWER = mecha_ins.getc();
+             beltL_TRIGGER = 1;
+            break;
+        case 0xB2 : 
+            cntL = mecha_ins.getc();
+            stateL_num = 1;
+            break;
+        case 0xB3 : 
+            ANGL_DES = mecha_ins.getc();
+            break;
+        case 0xAA : 
+            beltR_POWER = mecha_ins.getc();
+            beltR_TRIGGER = 1;
+            pid_off_R = 1;
+            kick_R = 1;
+            break;
+        case 0xAB : 
+            beltR_POWER = mecha_ins.getc();
+            beltR_TRIGGER = 1;
+            kick_R = 1;
+            break;
+        case 0xBA : 
+            beltL_POWER = mecha_ins.getc();
+            beltL_TRIGGER = 1;
+            pid_off_L = 1;
+            kick_L = 1;
+            break;
+        case 0xBB : 
+            beltL_POWER = mecha_ins.getc();
+            beltL_TRIGGER = 1;
+            kick_L = 1;
+            break;
+        case 0xAF : 
+            beltR_POWER = mecha_ins.getc();
+            beltR_TRIGGER = 1;
+            pid_off_R = 1;
+            break;
+        case 0xBF : 
+            beltL_POWER = mecha_ins.getc();
+            beltL_TRIGGER = 1;
+            pid_off_L = 1;
+            break;
+        case 0xFF : 
+            led_command = mecha_ins.getc();
+            break;
+    }
+    
+    while(mecha_ins.readable()){
+        char dummy = mecha_ins.getc();
+    }
+}
+
+void beltR_reset(){
+    if(BELTR_flag){
+        beltR_RE.reset();
+        BELTR_flag = 0;
+    }
+}
+
+//MD用I2C通信処理
+void md_setoutput(unsigned char address, int rotate, int duty){
+    char data[2];
+
+    // モーター駆動制御モード
+    switch (rotate) {
+        case CW:    data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
+        case CCW:   data[0] = 'R'; break;
+        case STOP:  data[0] = 'S'; break;
+    
+        case bCW:   data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
+        case bCCW:  data[0] = 'r'; break;
+        case bSTOP: data[0] = 's'; break;
+    
+        case sCW:   data[0] = 'A'; break; // 速度比例制御モード
+        case sCCW:  data[0] = 'B'; break;
+        case sSTOP: data[0] = 'T'; break;
+    
+        case shCW:   data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
+        case shCCW:  data[0] = 'b'; break;
+    
+        case lCW:   data[0] = 'L'; break; // 通常LAP
+        case lCCW:  data[0] = 'M'; break;
+        case lSTOP: data[0] = 'N'; break;
+        case lsCW:   data[0] = 'l'; break; // 速調LAP
+        case lsCCW:  data[0] = 'm'; break;
+        case lsSTOP: data[0] = 'n'; break;
+    
+        case rON: data[0] = 'P'; break; //リレー駆動モード
+        case rOFF: data[0] = 'p'; break;
+    }
+    
+    data[1] = duty & 0xFF;
+    md_i2c_tx.write(address << 1, data, 2); //duty送信
+}
\ No newline at end of file