File content as of revision 1:34371ffd3dc0:

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




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


////////////定数
int solution=1000;
double c_degree=0.36;   //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();
    reset();
    while(1) {
        can_receive();
        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);

    switch_ro.mode(PullUp);
    switch_ri.mode(PullUp);

    servo.init();
    timer.start();
}

void reset()
{
    while(switch_ro.read()) {
        out_ro(0.05);
    }
     ec_ro.reset();
     out_ro(0);
    while(switch_ri.read()) {
        out_ri(0.05);
    }
    
     ec_ri.reset();
     out_ri(0);
}

void pid(double target_ro_,double target_ri_)
{
    posi_ro=(ec_ro.getCount()%solution)*c_degree;
    if(posi_ro<0)posi_ro+=360;
    posi_ri=(ec_ri.getCount()%solution)*c_degree;
    if(posi_ri<0)posi_ri+=360;

    double now=timer.read();
    double d_time=now-pre_time;

    double deviation_ro=fabs(target_ro_)-posi_ro;
    if(fabs(deviation_ro)<90) { //そのまま
    } else if(deviation_ro>270) {
        deviation_ro-=360;
    } else if(deviation_ro<-270) {
        deviation_ro+=360;
    } else if(target_ro_>0) {
        if(deviation_ro<0)deviation_ro+=360;
    } else {
        if(deviation_ro>0)deviation_ro-=360;
    }

    double deviation_ri=fabs(target_ri_)-posi_ri;
    if(fabs(deviation_ri)<90) { //そのまま
    } else if(deviation_ri>270) {
        deviation_ri-=360;
    } else if(deviation_ri<-270) {
        deviation_ri+=360;
    } else if(target_ri_>0) {
        if(deviation_ri<0)deviation_ri+=360;
    } else {
        if(deviation_ri>0)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.1;

    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.1;

    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[2]&0b1111)<<8) - 360;
                target_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360;
                hand_mode= msg.data[3];

                break;
            }
            led2=1;
        } else led2=0;
    }
}