File content as of revision 0:5d4705b2893c:

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

#define PI 3.141592653589

Bcon mycon(fepTX, fepRX, fepad);
Serial pc(pcTX, pcRX, 115200);
Serial RS485(PC_10,PC_11,115200);


uint8_t b[8];
int16_t stick[4];
double engine[4];
double speed[6];
double spin_power;
double posiX , posiY , posiTheta;

DigitalOut stop(A3);
ikarashiMDC motor[] = {
    ikarashiMDC(0,0,SM,&RS485),
    ikarashiMDC(0,1,SM,&RS485),
    ikarashiMDC(0,2,SM,&RS485),
    ikarashiMDC(0,3,SM,&RS485),
};

OmniWheel omni(4);
OmniPosition posi(mwTX, mwRX);

PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！

/*プロトタイプ宣言*/
void chassis();//足回りの制御・ジャイロ・テラターム
void spin(double ang);//PID

int main(void){
    mycon.StartReceive();

    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(-180,180);
    angle.setOutputLimits(-0.4,0.4);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    while(1){
        stop = 1;
        chassis();
    }
    
}

void chassis(){
        /*非常停止*/
        //stop = 1;

        /*ジャイロ読み取り*/
        posiX = posi.getX();
        posiY = posi.getY();
        posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
        pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
        /*コントローラー受信*/
        for (int i=0; i<8; i++) b[i] = mycon.getButton(i);     
        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
        
        for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
        pc.printf(" | ");
        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
        pc.printf(" | ");
        if (mycon.status) pc.printf("received\r\n");
        else pc.printf("anything error...\r\n");

        for(int i=0; i<4; i++){
            speed[i] = 0;
        }
        
        /*notcontoroler*/
        for(int i=0; i<4; i++){
            motor[i].setSpeed(0);
        }
        /*全方位*/
        for(int i=0; i<4; i++){
            if(abs(stick[i]) > 10){
            engine[i] = 0.4*(stick[i]/128.0);
            }else{
                engine[i] = 0;
            }
        }

        /*旋回*/
        if(abs(stick[2]) > 10){
            spin_power = engine[2];
        }else{
            spin_power = 0;
        }
        
        omni.computeXY(engine[0],engine[1],-spin_power);
        for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];


        for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
        /*PID*/
        /*スパゲッティコードで申し訳ないです...*/
        if(abs(stick[2]) < 10){
        if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
            spin(0);
        }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
            spin(15);
        }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){
            spin(30);
        }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){
            spin(45);
        }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){
            spin(60);
        }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){
            spin(75);
        }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){
            spin(90);
        }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){
            spin(105);
        }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){
            spin(120);
        }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){
            spin(135);
        }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){
            spin(150);
        }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){
            spin(165);
        }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){
            spin(180);
        }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){
            spin(-165);
        }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){
            spin(-150);
        }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){
            spin(-135);
        }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){
            spin(-120);
        }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){
            spin(-105);
        }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){
            spin(-90);
        }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){
            spin(-75);
        }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){
            spin(-60);
        }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){
            spin(-45);
        }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){
            spin(-30);
        }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
            spin(-15);
        }else{
        }
        }
}
void spin(double ang)
{
    angle.setSetPoint(ang);
        while(1) {
        stop = 1;
        posiTheta = posi.getTheta()*(180.0/M_PI);
        angle.setProcessValue(posiTheta);
        pc.printf("spinning... |%d|%.2f|%.3f\r\n",ang,posiTheta,-angle.compute());
        for(int i=0; i<4; i++) motor[i].setSpeed(-angle.compute());
        //for(int i=4; i<8; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
        if ((fabs(ang - posiTheta) < 1)||(fabs(ang - posiTheta)> 7.5)) return;
    }
}