File content as of revision 12:486068800862:

#include "mbed.h"
#include "pin_config.h"
#include "wheel_unit.h"
#include "slider.h"
#include "PID_controller.h"
#include "controller.h"

DigitalOut powerSwitch(MDstop);
DigitalOut RS485Controller(PWM1);
Serial RS485(MDTX, MDRX, RS485_BAUD);

WheelUnit quadOmni(&RS485Controller, &RS485);
Slider slider(&RS485Controller, &RS485);
PIDC pid(HMCsda, HMCscl, KC, TI, TD, INTERVAL);
Controller pad(XBee1TX, XBee1RX, ADDR);
ikarashiMDC fukuda(&RS485Controller, 1, 1, SM, &RS485);
ikarashiMDC crab(&RS485Controller, 1, 2, SM, &RS485);
ikarashiMDC joint(&RS485Controller, 1, 3, SM, &RS485);

volatile bool receiveSuccessed;

Serial pc(USBTX, USBRX, 115200);
DigitalOut debugLED[3] = {
    DigitalOut(DebugLED3),
    DigitalOut(DebugLED4),
    DigitalOut(DebugLED5)
};

void getpad()
{
    while(true) {
        receiveSuccessed = pad.receiveState();
        pc.printf("%d\n\r", receiveSuccessed);
        wait(0.0);
        debugLED[0] = receiveSuccessed;
    }
}

void getPID()
{
    debugLED[1] = receiveSuccessed;
    if(receiveSuccessed) {
        pid.confirm();
    }
}

void allStop()
{
    quadOmni.moveXY(0, 0, 0);
    slider.slide(0);
    fukuda.setSpeed(0);
    crab.setSpeed(0);
    joint.setSpeed(0);
}

void moveWheels()
{
    if(pad.getNorm(1) > 0.5) {
        pid.PID::setSetPoint((pad.getRadian(1) - PI / 2) * (180.0 / PI));
    }
    quadOmni.moveXY(
        pad.getStick(0) / 2.0,
        -pad.getStick(1) /2.0,
        pid.getCalculationResult()
    );
}

void moveMach()
{
//        if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
//        if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
//        if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
//
//        if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
//        if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
//        if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
//
        if(!pad.getButton1(5)) fukuda.setSpeed(0.6);
        if(!pad.getButton1(6)) fukuda.setSpeed(-0.6);
        if(pad.getButton1(5) && pad.getButton1(6)) fukuda.setSpeed(-0.0);

//
//        if(!pad.getButton1(2)) {
//            motor.destroy(DESTROY_MAX_SPEED);
//        } else {
//            motor.destroy(0);
//        }
//
       if(!pad.getButton2(1)) slider.release();
       if(!pad.getButton2(3)) powerSwitch = 0;
}

void moveMotors()
{
    if(receiveSuccessed) {
        moveWheels();
        moveMach();
        debugLED[2] = receiveSuccessed;
    } else {
        allStop();
    }
}

void LChika()
{
    for(int i = 0; i < 3; i++) {
        debugLED[i] = true;
        wait(0.1);
        debugLED[i] = false;
    }
}

int main()
{
    LChika();
    fukuda.braking = true;
    crab.braking = true;
    joint.braking = true;
    receiveSuccessed = false;
    powerSwitch = true;
    LChika();

    int error = 0;
    while(true) {
        wait(INTERVAL);
        getpad();
        getPID();
        moveMotors();

        error++;
        if(receiveSuccessed) error = 0;
        if(error > 10) powerSwitch = 0;
    }
}