naoki tanabe / Mbed OS 2018NHK_gakugaku_robo

前のやつです。

Dependencies:   FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel

gakuBot/gakubot.cpp

Committer:
tanabe2000
Date:
2018-08-05
Revision:
1:af8bee219a3a
Child:
3:8f4c81ad256a

File content as of revision 1:af8bee219a3a:

#include "gakubot.h"

GakuBot::GakuBot() :
    led(LED2),
    pid(KP,KI,KD,RATE),
    omni(4),
    angle1_1(air1_0),angle1_2(air1_1),
    angle2_1(air2_0),angle2_2(air2_1),
    con(XBee2TX, XBee2RX, ADDR),
    wheel1 (PA_11,PA_12,NC, 2048, QEI::X4_ENCODING),
    RS485control(PA_4),
    RS485(PC_10,PC_11,115200),
    debugpc(USBTX,USBRX,115200),
    wheels( {
    ikarashiMDC(&RS485control,0,0,SM,&RS485),
    ikarashiMDC(&RS485control,0,1,SM,&RS485),
    ikarashiMDC(&RS485control,0,2,SM,&RS485),
    ikarashiMDC(&RS485control,0,3,SM,&RS485)
}),
fire( {
    ikarashiMDC(&RS485control,1,0,SM,&RS485),
    ikarashiMDC(&RS485control,1,1,SM,&RS485),
    ikarashiMDC(&RS485control,1,2,SM,&RS485)
})
{
    stick[4] = {0}, speed[4] = {0}, Output_PID = 0;
    airFlag = 0,airFlag2 = 0,airStatus = 0,airStatus2 = 0;
    int distance = 970,distanceOfset = 0;
    omni.wheel[0].setRadian(PI / 4.0 * 1.0);
    omni.wheel[1].setRadian(PI / 4.0 * 3.0);
    omni.wheel[2].setRadian(PI / 4.0 * 5.0);
    omni.wheel[3].setRadian(PI / 4.0 * 7.0);
    for(int i = 0; i < 4; i++) {
        wheels[i].braking = true;
    }
    for(int i = 0; i < 3; i++) {
        armMotor[i].braking = true;
    }
    pid.setMode(AUTO_MODE);
    pid.setInputLimits(-100, 1600);
    pid.setOutputLimits(-1.0, 1.0);
//    pid.setSetPoint(distance);// 目標値
    pid.setBias(0.0);   // outputの変わり目
}

void GakuBot::botConfirm()
{

}
void GakuBot::controllerMode1()
{

}
void GakuBot::autoMode1()
{

}


void GakuBot::controllMech()
{

}

void GakuBot::autoMech()
{

}