File content as of revision 0:411ab20ce87d:

#include "mbed.h"
#include "pin.h"



////////////関数
void setup();
void can_receive();
void pid(double,double);
void out_ro(double);
void out_ri(double);


////////////定数
int solution=500;
int c_degree=360/500;   //solution=500

double Kp_ro=0.01;
double Ki_ro=0;
double Kd_ro=0;
double Kp_ri=0.01;
double Ki_ri=0;
double Kd_ri=0;




////////////変数
double target_ro=0,target_ri=0;
bool hand_mode=0;
double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0;
double posi_ro=0,posi_ri=0;
double pre_time=0;


Timer timer;




//////////////////////////////////////////////
int main() {
    
    setup();
    while(1) {
        pid(target_ro,target_ri);
        wait(0.01);
    }
}

//////////////////////////////////////////////
void setup(){
    can.frequency(1000000);
    motor_ro_f.period_us(100);
    motor_ro_b.period_us(100);
    motor_ri_f.period_us(100);
    motor_ri_b.period_us(100);
    
    switch1.mode(PullUp);
    switch2.mode(PullUp);
    
    servo.init();
    timer.start();
}


void pid(double target_ro_,double target_ri_){
    posi_ro=(ec_ro.getCount()%solution)*c_degree;
    posi_ri=(ec_ri.getCount()%solution)*c_degree;
    
    double now=timer.read();
    double d_time=now-pre_time;
    
    double deviation_ro=fabs(target_ro_)-posi_ro;
    if(target_ro_>0){
        if(deviation_ro<0)deviation_ro=360+deviation_ro;
    }else{
        if(deviation_ro>0)deviation_ro=deviation_ro-360;
    }
    double deviation_ri=fabs(target_ri_)-posi_ri;
    if(target_ri_>0){
        if(deviation_ri<0)deviation_ri=360+deviation_ri;
    }else{
        if(deviation_ri>0)deviation_ri=deviation_ri-360;
    }
    
    pile_ro+=deviation_ro;
    pile_ri+=deviation_ri;
    
    out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro *  Ki_ro * d_time);
    out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri *  Ki_ri * d_time);
    
    distance_ro_old=deviation_ro;
    distance_ri_old=deviation_ri;
    pre_time=now;
}


void out_ro(double duty){
    double dutylimit=0.4;

    if(duty > 0) { //入力duty比が正の場合
        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
        if( fabs( duty ) < dutylimit ) { //制限値内
            motor_ro_f = fabs(duty);
            motor_ro_b = 0;
        } else { //制限値超
            motor_ro_f = dutylimit;
            motor_ro_b = 0;
        }
    } else {//入力duty比が負の場合
        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
        if( fabs(duty) < dutylimit) { //制限値内
            motor_ro_f = 0;
            motor_ro_b = fabs(duty);
        } else { //制限値超
            motor_ro_f = 0;
            motor_ro_b = dutylimit;
        }
    }
    //pre_out_r=duty;
}


void out_ri(double duty){
    double dutylimit=0.4;

    if(duty > 0) { //入力duty比が正の場合
        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
        if( fabs( duty ) < dutylimit ) { //制限値内
            motor_ri_f = fabs(duty);
            motor_ri_b = 0;
        } else { //制限値超
            motor_ri_f = dutylimit;
            motor_ri_b = 0;
        }
    } else {//入力duty比が負の場合
        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
        if( fabs(duty) < dutylimit) { //制限値内
            motor_ri_f = 0;
            motor_ri_b = fabs(duty);
        } else { //制限値超
            motor_ri_f = 0;
            motor_ri_b = dutylimit;
        }
    }
    //pre_out_r=duty;
}

//////////////////////////////////////////can
void can_receive(){
    CANMessage msg;
    for(int i=0; i<5; i++) {
        if(can.read(msg)) {
            if(msg.id==0) {
                target_ro= msg.data[0] | ((msg.data[1]&0b1111)<<8) - 360;
                target_ri= msg.data[2] | ((msg.data[1]&0b11110000)<<4) - 360;
                hand_mode= msg.data[3];
                
                break;
            }
                led2=1;
        } else led2=0;
    }
}