hirromu takaki / wheelUnit

手動ｂ足回り

Dependents:   kobayashi_rei

wheelUnit.cpp

Committer:
THtakahiro702286
Date:
2019-09-09
Revision:
3:01a6eca21b23
Parent:
2:9017bbe177b7

File content as of revision 3:01a6eca21b23:

#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(1,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::setBrake(int trigger)
{
    brake = (255 - trigger) / 255.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] * brake);
    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");
}