yattaze

Dependencies:   omni_all ikarashiMDC

main.cpp

Committer:
THtakahiro702286
Date:
2019-05-02
Revision:
0:00d1839224b6

File content as of revision 0:00d1839224b6:

#include "mbed.h"
#include "omni_all.h"
#include "ikarashiMDC.h"

Serial pc(USBTX, USBRX, 115200);
Serial serial(PC_6,PC_7);
DigitalOut serialcontrol(D2);
Omni_all omni(3);   //オムニホイールの単位円上の位置を入力
Timer rad;

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

int main()
{
    double acc[3];
    serial.baud(115200);
    ikarashi[0].braking = true;
    for(int i = 0; i < 3; i++)
    {
        omni.sendRad(PI * (1+4*i)/6, i);
    }
    rad.start();
    while(1)
    {
        if(rad >= (PI * 2))
        {
            rad.reset();
        }
        for(int i = 0; i < 3; i++)
        {
            acc[i] = omni.vec(rad,i);
        }
        for(int i = 0; i < 3;i++)
        {
            ikarashi[i].setSpeed(acc[i] / 2);
        }
        pc.printf("%f,%f,%f\r\n",acc[0],acc[1],acc[2]);
    }
}