File content as of revision 0:a1f453dd2bb5:

#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送信
}