File content as of revision 0:377bc90d362c:

#include "mbed.h"           //wheel_test6
#include "ikarashiMDC.h"    //sampleライブラリ -> 成功
#include "omni_wheel.h"
#include "sample02.h"
#include "OmniPosition.h"

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 sw(PC_2);


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),
};

Sample sample(1500, 1500, 0.4, 0.14);


double sign(double a){
    return(a>0)-(a<0);
}


int main()
{
    motor[0].braking = true;
    double valueX, valueY, value[4];
    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
    omni.wheel[1].setRadian(PI * 3.0 / 4.0);
    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
    omni.wheel[3].setRadian(PI * 7.0 / 4.0);
    sample.target_xy(0, 4000, posi.getX(), posi.getY());
    while(1){
        sw = 1;
        sample.calculate(posi.getX(), posi.getY());
        valueX = sample.getvalue_x();
        valueY = sample.getvalue_y();
        omni.computeXY(valueX,valueY,0);
         
        pc.printf("x:%d || y:%d || a:%f || vector:%f ||   ", posi.getX(), posi.getY(), posi.getTheta(), sample.vector);
        for(int i = 0; i < 4; i++){
            value[i] = omni.wheel[i];
            pc.printf("%.2f || ",value[i]);
        }
        int a = sample.nowDis > (sample.accdis+sample.middledis);
        pc.printf(" now:%f || %f\r\n",sample.nowDis, sample.counter);
        for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
    }
}