File content as of revision 1:93f6f64ac719:

#include "mbed.h"           //wheel_test7
#include "ikarashiMDC.h"    //protoライブラリ+PID -> うひょひょひょ
#include "omni_wheel.h"
#include "proto01.h"
#include "OmniPosition.h"
#include "PID.h"

PID angle(2.0,0.0,0.00001,0.01);
OmniWheel omni(4);
OmniPosition posi(PC_12, PD_2);
Serial serial(PC_6, PC_7, 115200);
DigitalOut serialcontrol(D10);
RawSerial pc(USBTX, USBRX, 115200);
DigitalOut sw1(PC_2);
DigitalOut sw2(PC_3);

ikarashiMDC motor[]=
{
    ikarashiMDC(&serialcontrol,0,0,SM,&serial),
    ikarashiMDC(&serialcontrol,0,1,SM,&serial),
    ikarashiMDC(&serialcontrol,0,2,SM,&serial),
    ikarashiMDC(&serialcontrol,0,3,SM,&serial),
};

Proto1 proto(500, 1000, 0.4, 0.14);

int main()
{
    sw1 = 1;
    sw2 = 2;
    int cnt=0;
    motor[0].braking = true;
    double valueX, valueY, value[4];
    omni.wheel[0].setRadian(PI * 1 / 4);
    omni.wheel[1].setRadian(PI * 3 / 4);
    omni.wheel[2].setRadian(PI * 5 / 4);
    omni.wheel[3].setRadian(PI * 7 / 4);
    angle.setInputLimits(-3.14,3.14);
    angle.setOutputLimits(-0.4,0.4);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    while(1){
        
        if (cnt==500) prot.target_xy(-3800, 0, posi.getX(), posi.getY());
        if (cnt==1000) proto.target_xy(0, 0, posi.getX(), posi.getY());
        if (cnt==1500) proto.target_xy(-2000, 0, posi.getX(), posi.getY());
        if (cnt==2000) proto.target_xy(-2000, 2700, posi.getX(), posi.getY());
        if (cnt==2500) proto.target_xy(-2000, 0, posi.getX(), posi.getY());
        if (cnt==3000) proto.target_xy(0, 0, posi.getX(), posi.getY());
        angle.setProcessValue(posi.getTheta());
        double spin_power = -angle.compute();
        proto.calculate(posi.getX(), posi.getY());
        omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
        
        for(int i = 0; i < 4; i++){
            value[i] = omni.wheel[i];
            pc.printf("%.2f || ",value[i]);
        }
        pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
        , posi.getX(), posi.getY(), posi.getTheta(), proto.vector, proto.nowDis, proto.counter);
        
        for(int i = 0; i < 4; i++) {
            if (fabs(value[i]) < 0.05) value[i] = 0;
            motor[i].setSpeed(value[i]);
        }
        cnt++;
        pc.printf("||pid:%.2f\r\n",spin_power);
    }
}