File content as of revision 2:8d16db0d55b7:

#include "mbed.h"           //wheel_test4
#include "controller.h"
#include "ikarashiMDC.h"
#include "R1370.h"
#include "omni_wheel.h"
#include "PID.h"
#define PI 3.141593

PID pid1(4.5,100.0,0.00000,0.01);
OmniWheel omni(4);
Controller pad(PC_10, PC_11, 208);
R1370 R1370(PB_6,PA_10);
Serial pc(USBTX, USBRX, 115200);
Serial serial(PC_6, PC_7, 115200);
DigitalOut serialcontrol(D10);
//DigitalOut reset(PC_9);


ikarashiMDC motor[]=
{
    ikarashiMDC(&serialcontrol,2,0,SM,&serial),
    ikarashiMDC(&serialcontrol,2,1,SM,&serial),
    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
    ikarashiMDC(&serialcontrol,2,3,SM,&serial),
};


int main()
{
//    reset = 1;
    motor[0].braking = true;
    int b[11];
    double rad[2], dis[2], value[3], X, Y, set_value, original_angle, now_angle, start_angle;
    pid1.setInputLimits(-180,180);
    pid1.setOutputLimits(-0.3,0.3);
    pid1.setBias(0);
    pid1.setMode(1);
    pid1.setSetPoint(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);
    while(1){
        if(pad.receiveState()){
            for(int i = 0; i < 13; i++){
                b[i] = pad.getButton1(i);
            }
            for(int i = 0; i < 2; i++){
                rad[i] = pad.getRadian(i);
                dis[i] = pad.getNorm(i);
                rad[i] = PI - rad[i];
            }
            X = dis[0] * cos(rad[0]);
            Y = dis[0] * sin(rad[0]);
            R1370.update();
            /*
            ここまでテンプル
            */
            start_angle = R1370.getAngle();
            /*
            旋回
            */
            if(b[7]==0)set_value++;
            if(b[9]==0)set_value--;
            set_value %= 4;
            switch(((set_value & 4) + 4) & 4){
                case 1: original_angle = 90;break;
                case 2: original_angle = 179;break;
                case 3: original_angle = -90;break;
                default: original_angle = 0;break;
            /*
            リセット
            */
            if(b[6] == 0){
                original_angle=angle;
            }
            now_angle = start_angle - original_angle;
            if(now_angle < -180)now_angle = now_angle + 360;
            pid1.setProcessValue(now_angle);
            double spin_power = pid1.compute();
            omni.computeXY(X,Y,-spin_power);
            pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power);
            for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
            
        }else{
            pc.printf("error\n\r");
            for (int i = 0; i < 4; i++)motor[i].setSpeed(0);
        }
    }
}