File content as of revision 1:86c4c38abe40:

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



////////////関数
void setup();
void can_send();
void pid(double,double);
void out_lo(double);
void out_li(double);

////////////定数
int solution=1000;
double c_degree=0.36;   //solution=500

double Kp_lo=0.01;
double Ki_lo=0;
double Kd_lo=0;
double Kp_li=0.01;

double Ki_li=0;
double Kd_li=0;



////////////変数
double target_ro=0,target_ri=0;
double target_lo=0,target_li=0;
bool hand_mode=0;
double distance_lo_old=0,distance_li_old=0,pile_lo=0,pile_li=0;
double posi_lo=0,posi_li=0;
double pre_time=0;

Timer timer;




/////////////////////////////////////////////
int main()
{

    setup();
    
    while(1) {
       can_send();
       pid(target_lo,target_li);
        wait(0.01);
    }
}


void setup()
{
    can1.frequency(1000000);
    motor_lo_f.period_us(100);
    motor_lo_b.period_us(100);
    motor_li_f.period_us(100);
    motor_li_b.period_us(100);

    hand.mode(PullUp);
    switch2.mode(PullUp);
    switch3.mode(PullUp);
    switch4.mode(PullUp);


    device.baud(115200);
    device.format(8,Serial::None,1);
    device.attach(dev_rx, Serial::RxIrq);
    wait(0.05);
    theta0=degree0;
    check_gyro();
    timer.start();
}


//////////////////////////////////////can
void can_send()
{
    char data[4]= {0};
    int target_ro_send=target_ro+360;
    int target_ri_send=target_ri+360;
    data[0]=target_ro_send & 0b11111111;
    data[1]=target_ri_send & 0b11111111;
    data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000);
    data[3]=hand_mode;

    if(can1.write(CANMessage(0,data,4)))led4=1;
    else led4=0;
}


void out_lo(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_lo_f = fabs(duty);
            motor_lo_b = 0;
        } else { //制限値超
            motor_lo_f = dutylimit;
            motor_lo_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_lo_f = 0;
            motor_lo_b = fabs(duty);
        } else { //制限値超
            motor_lo_f = 0;
            motor_lo_b = dutylimit;
        }
    }
    //pre_out_r=duty;
}


void out_li(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_li_f = fabs(duty);
            motor_li_b = 0;
        } else { //制限値超
            motor_li_f = dutylimit;
            motor_li_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_li_f = 0;
            motor_li_b = fabs(duty);
        } else { //制限値超
            motor_li_f = 0;
            motor_li_b = dutylimit;
        }
    }
    //pre_out_r=duty;
}

void pid(double target_lo_,double target_li_)
{
    posi_lo=(ec_lo.getCount()%solution)*c_degree;
    if(posi_lo<0)posi_lo+=360;
    posi_li=(ec_li.getCount()%solution)*c_degree;
    if(posi_li<0)posi_li+=360;

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

    double deviation_lo=fabs(target_lo_)-posi_lo;
    if(fabs(deviation_lo)<90) { //そのまま
    } else if(deviation_lo>270) {
        deviation_lo-=360;
    } else if(deviation_lo<-270) {
        deviation_lo+=360;
    } else if(target_lo_>0) {
        if(deviation_lo<0)deviation_lo+=360;
    } else {
        if(deviation_lo>0)deviation_lo-=360;
    }

    double deviation_li=fabs(target_li_)-posi_li;
    if(fabs(deviation_li)<90) { //そのまま
    } else if(deviation_li>270) {
        deviation_li-=360;
    } else if(deviation_li<-270) {
        deviation_li+=360;
    } else if(target_li_>0) {
        if(deviation_li<0)deviation_li+=360;
    } else {
        if(deviation_li>0)deviation_li-=360;
    }

    pile_lo+=deviation_lo;
    pile_li+=deviation_li;

    out_lo(deviation_lo * Kp_lo + (posi_lo - distance_lo_old) / d_time * Kd_lo + pile_lo *  Ki_lo * d_time);
    out_li(deviation_li * Kp_li + (posi_li - distance_li_old) / d_time * Kd_li + pile_li *  Ki_li * d_time);

    distance_lo_old=deviation_lo;
    distance_li_old=deviation_li;
    pre_time=now;
}