hirromu takaki / wheelUnit

手動ｂ足回り

Dependents:   kobayashi_rei

wheelUnit.cpp

Committer:
THtakahiro702286
Date:
2019-07-11
Revision:
2:9017bbe177b7
Parent:
1:3c9eaf598f21
Child:
3:01a6eca21b23

File content as of revision 2:9017bbe177b7:

#include "wheelUnit.h"

WheelUnit::WheelUnit(Serial* serial) :
    omni(4),
    pc(USBTX, USBRX, 115200)
{
    md = new ikarashiMDC*[4];
    for(int i = 0; i < 4; ++i) md[i] = new ikarashiMDC(2,i,SM,serial);
    for(int i = 0; i < 4; ++i) md[i]->braking = true;
    for(int i = 0; i < 4; i++) omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
}

void WheelUnit::move(float x,float y,float turnPower)
{
    omni.computeXY(x,y,turnPower);
    for (int i = 0; i < 4; ++i) wheel[i] = omni.wheel[i];
    for (int i = 0; i < 4; ++i) md[i]->setSpeed(wheel[i] * SPEEDRATE);
    print = omni.wheel[0];
    pc.printf("%f\n\r",print);
}

void WheelUnit::stop()
{
    for (int i = 0; i < 4; ++i) md[i]->setSpeed(0);
    pc.printf("0\n\r");
}