File content as of revision 0:2b93017d0053:

#include "mbed.h"
#include "ikarashiMDC.h"
#include "ikarashiSV.h"
#include "controller.h"
#include "pinconfig.h"
#include "omni_wheel.h"
#include "Servo.h"
#include "math.h"
#include "position_controller.h"
#include "OmniPosition.h"
#include "PID.h"

#define PI 3.141592653589

Bcon mycon(fepTX, fepRX, fepad);
Serial pc(pcTX, pcRX, 115200);
DigitalOut stop(em_stop);
ikarashiSV  slvT(slv1,slv2, slv3,slv4);
ikarashiSV2 slvU(slv5,slv6);
Servo servo(servo1);
//DigitalOut test3(PA_5);
//DigitalOut test4(PB_10);
//DigitalOut test1(PB_5);
//DigitalOut test2(PB_4);

//PID angle(2.0,0.0,0.00001,0.01);
PID angle(10.0, 0.0, 0.001, 0.01);
OmniWheel omni(4);
OmniPosition posi(mwTX, mwRX);
Serial RS485(mdcTX,mdcRX,115200);
ikarashiMDC motor[] = {
    ikarashiMDC(0,0,SM,&RS485),/*足*/
    ikarashiMDC(0,1,SM,&RS485),/*足*/
    ikarashiMDC(0,2,SM,&RS485),/*足*/
    ikarashiMDC(0,3,SM,&RS485),/*足*/
    ikarashiMDC(1,0,SM,&RS485),/*腕昇降*/
    ikarashiMDC(1,1,SM,&RS485),/*腕前後*/
};

PositionController pcon[] = {
    PositionController(200.0, 200.0, 0.2, 0.0, 0.6),
    PositionController(500.0, 500.0, 0.1, 0.0, 0.20),
    PositionController(1.0, 13.0, 0.14, 0.05, 0.15),
    PositionController(100.0, 100.0, 0.11, 0.0, 0.20),
    PositionController(300, 400, 0.1, 0.0, 0.2),
    PositionController(700, 1000, 0.1, 0.0, 0.25),
    PositionController(1500.0, 1500.0, 0.1, 0.0, 0.30),
    PositionController(200, 200, 0.1, 0.0, 0.20)
};

void add(){
//    slv.add_state();
}


int main(void){

    mycon.StartReceive();
    uint8_t b[8];
    int16_t stick[4];
    double accele[4];
    double speedX, speedY, speed[6];
    double spin_power;
    double X, Y, R;

    int val;
    long int xycnt=0, svcnt=0;

    servo.calibrate(0.00095, 90);
    servo.write(0);

    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
    omni.wheel[1].setRadian(PI * 3.0 / 4.0);
    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
    omni.wheel[3].setRadian(PI * 7.0 / 4.0);
    
    angle.setInputLimits(-3.14,3.14);
    angle.setOutputLimits(-0.6,0.6);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    
    while (1)
    {
        for(int i=0; i<6; i++){
            speed[i] = 0;
        }
        
        // 非常停止
        stop = 1;
        
        // コントローラー受信
        for (int i=0; i<8; i++) b[i] = mycon.getButton(i);     
        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
        
        // 腕昇降
        if(b[0] != 0){
            speed[4] = 0.2;
        }else if(b[2] != 0){
            speed[4] = -0.2;
        }else{
            speed[4] = 0;
        }

        // 腕前後
        if(b[3] != 0){
            speed[5] = 0.2;
        }else if(b[1] != 0){
            speed[5] = -0.2;
        }else{
            speed[5] = 0;
        }
        
        // 計測輪
        X = posi.getX();
        Y = posi.getY();
        R = posi.getTheta();
        
        // PID
        angle.setProcessValue(R);
        spin_power = -angle.compute();
        
        // s字制御
        if (xycnt==300) pcon[0].targetXY(0, -1000);
        pcon[0].compute(X, Y);
        spin_power = -angle.compute();  
        omni.computeXY(pcon[0].getVelocityX(), pcon[0].getVelocityY(), spin_power);
        for (int i = 0; i < 4; i++) speed[i] = omni.wheel[i];

        // 非常停止テスト用
//        for (int i=0; i<4; i++) speed[i] = spin_power;
//        for (int i=0; i<4; i++) speed[i] = 0;
        
        // モーター出力
        for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
        
        // 以下ソレノイド
        if ((X > -50 && X < 50) && (Y > -1050 && Y < -950)) svcnt++;
//        svcnt++;
        if (svcnt == 300)  slvU.solenoid(1);
        if (svcnt == 500)  slvT.solenoid(1);
        if (svcnt == 600)  slvT.solenoid(2);
        if (svcnt == 800) slvT.solenoid(0);
        if (svcnt == 1000) slvU.solenoid(0);
        
        // イカサーボ
        if(b[7] != 0){
            servo.write(0.3);
        }else{
            servo.write(0);
        }
        
        // 表示部
        // コントローラ
        /*
        if (mycon.status) {
            pc.printf("received");
//            for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
            pc.printf("%d ", mycon.sum);
            pc.printf(" | ");
            for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
            pc.printf(" | ");
        }
        else pc.printf("anything error... | ");
        */
        
        // 計測輪
        pc.printf("x: %5d | y: %5d | r: %3.2f | Vx: %f | Vy: %f "
        , posi.getX(), posi.getY(), posi.getTheta(), pcon[0].getVelocityX(), pcon[0].getVelocityY());
        
        // スピード
        pc.printf("spd: ");
        for (int i=0; i<6; i++) pc.printf("%d: %2.2f | ", i, speed[i]);
        
//        pc.printf("xycnt: %d | svcnt: %d", xycnt, svcnt);
        pc.printf("\r\n");
        xycnt++;
    }
    
}