File content as of revision 1:5c0b7355adcd:

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

PID pid1(3.0,3.0,0.000005,0.01);
OmniWheel omni(3);
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);

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

int main()
{
    pid1.setInputLimits(-90,90);
    pid1.setOutputLimits(-0.5,0.5);
    pid1.setBias(0);
    pid1.setMode(1);
    pid1.setSetPoint(0);
    omni.wheel[0].setRadian(PI * 1.0 / 6.0);
    omni.wheel[1].setRadian(PI * 5.0 / 6.0);
    omni.wheel[2].setRadian(PI * 9.0 / 6.0);
  
    motor[0].braking = true;
    int b[11];
    double rad[2],dis[2];
    double value[3];
    pc.printf("saaa");
    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];
            }
            double X = dis[0]*cos(rad[0]);
            double Y = dis[0]*sin(rad[0]);
            R1370.update();
            double angle = R1370.getAngle();
            pid1.setProcessValue(angle);
            double spin_power = pid1.compute();
            omni.computeXY(X,Y,-spin_power);
            pc.printf("%.3f||%.3f||%.3f||%.3f\n\r",X,Y,angle,spin_power);
            for(int i = 0; i < 3; i++)
                motor[i].setSpeed(omni.wheel[i]);
            
        }else{
            pc.printf("error\n\r");
            for (int i = 0; i < 3; i++)
            {
                motor[i].setSpeed(0);
            }
        }
    }
}