File content as of revision 1:0867d6d1421a:

#include "mbed.h"
#include"scrp_slave.hpp"
#include "rotary_inc.hpp"
#include"cmath"
#include"gy521.hpp"
#include "neopixel.h"


#define goal_x -15000.0
#define goal_y 14000.0
#define period_r 6000.0
NeoPixelOut npx(PB_4,7);
I2C i2d(PB_3,PB_10);
GY521 gyro(i2d);
ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定
RotaryInc v[4]{RotaryInc(PC_5,PA_12,1,256,4),RotaryInc(PA_9,PA_8,1,256,4),RotaryInc(PC_3,PC_2,1,256,4),RotaryInc(PC_11,PC_10,1,256,4)};//ロリコンピン
//PC_11 PC_10 PC_3 PC_2
int target[4];//target変数　この変数の値を目指すように動く
int angler=90;
int speedr=1024;
bool auto_swich = false;
bool limit_switch = true;
int step=0;
int auto_mode=0;


int check_tepu=0;

bool limi=true;

bool yabe=false;

DigitalIn limitB(PB_12,PullUp);




bool add(int id,int ppp){//速度を変えるやつ
    if(auto_swich==false&&limi==true){
        target[id]=30*ppp;
    }
    return true;
}

bool anglez(int id,int ppp){//おまけで作ったやつ　アングルを決めてその方向へ走る
    angler=ppp;
    for(int i=0;i<4;i++){
    //target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
    
    }    
    return true;
}

bool speedz(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
     speedr=ppp;
     for(int i=0;i<4;i++){
    target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
    
    }
    return true;
}

bool auto_on(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
    if(ppp==1){
    auto_swich=true;
    step=0;
    }
    return true;
}

bool auto_off(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
    if(ppp==1){
    auto_swich=false;
    step=0;
    }
    return true;
}

bool turn(int id,int ppp){//回転
    for(int i=0;i<4;i++){
    target[i]=ppp;   
    } 
    return true;
}

bool pro(int id,int ppp){  //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
    int angle=ppp%1000;
    int speed=ppp/1000;
    for(int i=0;i<4;i++){
    target[i]=(sin(M_PI/180.0*(angle+90*i))+cos(M_PI/180.0*(angle+90*i)))*speed*100.0;
    
    }
    return true;
}

bool limit2(int id,int ppp){
    if(ppp==1){
        limit_switch=false;
    }
    return true;
}

bool  limit(int a,int &b){
    return limit2(0,a);
}
bool  abc1(int a,int &b){
    check_tepu+=1;
    return add(0,a);
}
bool  abc2(int a,int &b){
    check_tepu+=1;
    return add(1,a);
}
bool  abc3(int a,int &b){
    check_tepu+=1;
    return add(2,a);
}
bool  abc4(int a,int &b){
    check_tepu+=1;
    return add(3,a);
}
bool  abc5(int a,int &b){
    return turn(0,a);
}
bool  abc6(int a,int &b){
    return pro(0,a);
}
bool  abc7(int a,int &b){
    return anglez(0,a);
}
bool  abc8(int a,int &b){
    return speedz(0,a);
}
bool  abc9(int a,int &b){
    return auto_on(0,a);
}
bool  abc10(int a,int &b){
    return auto_off(0,a);
}

bool  abc22(int a,int &b){
    yabe=true;
    return true;
}

int main() {
   // slave.addCMD(1,limit);
    slave.addCMD(2,abc1);
    slave.addCMD(3,abc2);
    slave.addCMD(4,abc3);
    slave.addCMD(5,abc4);
    slave.addCMD(6,abc22);
   // slave.addCMD(6,abc5);
   // slave.addCMD(7,abc6);
   // slave.addCMD(8,abc7);
   // slave.addCMD(9,abc8);
    slave.addCMD(12,abc9);
    slave.addCMD(13,abc10);
    slave.addCMD(1,limit);

    int i;    
    double x_period=0;
    double y_period=0;
    int after[4],before[4],before_parus[4],speed_pwm[4];
    double ca[4],P[4],I[4],D[4],integral[4];
    PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_6 PC_8
    PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_7 PC_9
    for(i=0;i<4;i++){
    motor_p[i].period_us(2048);
    motor_m[i].period_us(2048);
    target[i]=0;
    after[i]=0;
    before[i]=0;
    ca[i]=0;
    P[i]=0;
    I[i]=0;
    D[i]=0;
    before_parus[i]=0;
    integral[i]=0;
    speed_pwm[i]=0;
   }
    Timer name;
    Timer limilimi;
    limilimi.start();
    name.start();
    gyro.start();
    double angle=0;
    double goal_angle1=0;
    double test_speed_auto=0;
    int test_x=0;
    int test_y=0;
    int test_a=0;
    bool limilimi_swich=false;
    while(limit_switch==true) {
    angle=45.0+gyro.yaw;
    
    if(auto_swich==true){
            goal_angle1=-1.0*atan((goal_x-x_period)/(goal_y-y_period))/M_PI*180.0;
            if(goal_angle1>0){
                goal_angle1-=180.0;
            }else{
                goal_angle1+=180.0;
            }
            
        if(step==0&&((goal_angle1-gyro.yaw)>1||(goal_angle1-gyro.yaw)<-1)){
            test_speed_auto=(goal_angle1-gyro.yaw)*-20.0;
            if(test_speed_auto>2000){
                test_speed_auto=2000;    
            }else if(test_speed_auto<-2000){
                test_speed_auto=-2000;    
            }
            turn(0,(test_speed_auto)*1);
        }else if(step>=0&&step<20){
            step+=1;    
        }else if(step==20&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)||  (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){
            test_speed_auto=-1.0*(sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r))/10.0;
            if(test_speed_auto>1000){
            test_speed_auto=1000;    
            }else if(test_speed_auto<-1000){
                test_speed_auto=-1000;    
            }
            speedz(0,(test_speed_auto)*1);
            
        }else {
            auto_swich=false;
        }
    }
    
    
    
        if(limitB){
            limi=true;
            limilimi_swich=false;
        }else{
            int arc=0;
            for(i=0;i<4;i++){
                arc+=target[i]*sin((45.0+90.0*i)/90.0*M_PI);
            }
            if(arc>0){
                //target[i]=0;
            }
            if(limilimi_swich==false){
                slave.send1(255,20,2);
                limilimi_swich=true;
                limilimi.reset();
            }
        }
        if(limilimi.read_ms()<500&&!limitB){
            limi=false;
            for(i=0;i<4;i++){
                target[i] =0;   
            }
        }else {
            limi=true;
        }
    
        for(i=0;i<4;i++){
            after[i]=v[i].get();//ロリコンから値読み取り
            integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
            P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例
            D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分
            I[i]=0.0001*integral[i];//積分
            ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる
            if(ca[i]>0.4){ca[i]=0.4;}
            if(ca[i]<-0.4){ca[i]=-0.4;}
            motor_p[i]=ca[i];
            motor_m[i]=ca[i]*-1.0;
            speed_pwm[i]=after[i]-before[i];
            before_parus[i]=target[i]/10.0-(after[i]-before[i]);
            before[i]=after[i];
        }
        
        x_period+=(((speed_pwm[0]-speed_pwm[2])*cos(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*cos(M_PI/180.0*(angle+90.0))*(-1.0))/2);
        y_period+=(((speed_pwm[0]-speed_pwm[2])*sin(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*sin(M_PI/180.0*(angle+90.0))*(-1.0))/2);
        test_x=x_period;
        test_y=y_period;
        test_a=angle;
        slave.port2.printf("%d\n",1);
        //slave.send1(255,30,test_x);
        while(name.read_ms()<30){}
        //slave.send1(255,31,test_y);
        while(name.read_ms()<60){}
        //slave.send1(255,32,test_a);
        while(name.read_ms()<100){}
        
        for(i = 0; i < 4; i++){
        npx.global_scale = 0.05f;
        npx.normalize = false;
            if(check_tepu>0){
            npx.setPixelColor(i,0xFF0000);
            }else {
            npx.setPixelColor(i,0x0000FF);
            }
        }
        for(i = 4; i < 7; i++){
        npx.global_scale = 0.05f;
        npx.normalize = false;
        if(auto_swich){
            npx.setPixelColor(i,0xFFFFFF);
        }else {
            npx.setPixelColor(i,0x00FF00);
        }
        }
         npx.show();
        if(check_tepu==0){
            auto_mode+=1;
        }else {
            auto_mode=0;    
        }
        
        if(auto_mode>5){
            limit_switch=false;
        }
        
        check_tepu=0;
        
        gyro.update();
        name.reset();
    }
    
  
    while(1){
        for(i=0;i<4;i++){
        target[i]=0;
        motor_p[i]=0;
        motor_m[i]=0;
        
        npx.global_scale = 0.05f;
        npx.normalize = false;
        npx.setPixelColor(i,0x00FFFF);
        
        
        }
    npx.show();
    }
    
}