File content as of revision 11:10d3dc96c469:

#include "mbed.h"
#include "ikarashiMDC.h"
#include "pinconfig.h"
#include "omni_wheel.h"
#include "Servo.h"
#include "math.h"
#include "SerialMultiByte.h"
#include "PID.h"
#include "R1370.h"
#include "OmniPosition.h"
#include "FEP_RX22.h"
#include "cmath"
 
#define PI 3.141592653589
#define maxSpeed 0.4
 
DigitalIn userButton(USER_BUTTON);
AnalogIn VOLUME(A0);
 
 
FEP_RX22 mycon(fepTX, fepRX, fepad);
Serial pc(pcTX, pcRX, 115200);
Serial RS485(mdcTX,mdcRX,115200);
 
 
DigitalOut stop(em_stop);
 
SerialMultiByte rcv(sub2TX,sub2RX);
 
ikarashiMDC motor[] = {
    ikarashiMDC(0,0,SM,&RS485), //asi
    ikarashiMDC(0,1,SM,&RS485), //asi
    ikarashiMDC(0,2,SM,&RS485), //asi
    ikarashiMDC(0,3,SM,&RS485), //asi
    ikarashiMDC(1,0,SM,&RS485), //全体昇降1
    ikarashiMDC(1,1,SM,&RS485), //全体昇降2
    ikarashiMDC(1,2,SM,&RS485), //フジモン機構
    ikarashiMDC(1,3,SM,&RS485), //フジモン機構
    ikarashiMDC(2,0,SM,&RS485), //井上左昇降
    ikarashiMDC(2,1,SM,&RS485), //井上右昇降
    ikarashiMDC(2,2,SM,&RS485), //井上左前後
    ikarashiMDC(2,3,SM,&RS485), //井上右前後
};
 
Servo pwm_imput1(BLDC1);//ブラシレス宣言
Servo pwm_imput2(BLDC2);
Servo pwm_imput3(BLDC3);
// angとanglとangleの変数とクラス名がある。気をつけよう*********************重要重要***********************
uint8_t b[16];
int16_t stick[4];
int16_t trigger[4];
int16_t volume[3];
uint8_t toggle[4];
uint8_t timeout;
uint8_t data[128];
int pw;

double speed[12];
double engine[4];
double spin_power;
double posiX , posiY , posiTheta;

int TargetAngle = 0;

int sum_1 = 0;
int sum_2 = 0;

double bldcspeed = 0.8;

int16_t angle[4] = {0};//エンコーダ受信格納
uint8_t pulse[8] = {0};
 
OmniWheel omni(4);
OmniPosition posi(sub1TX, sub1RX);
 
PID angl(10.0, 5.0, 0.0000005, 0.01);
 
//プロトタイプ宣言
void chassis();         //機体自体の制御
void spin(double ang);  //PID
int  pm(double num);    //正負判定
 
Timer t;
 
int main(void){
     t.start();
    
    rcv.setHeaders(0xff,0xff);
    rcv.startReceive(4); //SerialMultiByte受信
    
    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);
    
    angl.setInputLimits(-180,180);
    angl.setOutputLimits(-0.4,0.4);
    angl.setBias(0);
    angl.setMode(1);
    angl.setSetPoint(0);
    while(1){
        stop = toggle[0];
        chassis();
    }
    
}
 
void chassis(){
        
 #if ControllerMode   
        for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
        for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
        
//        for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
//        pc.printf(" | ");
//        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
//        pc.printf(" | ");
//        for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
//        pc.printf(" | ");
#else

        /*ジャイロ読み取り*/
        /*足回りのXY座標は多分使わないので無くてもよし。*/
        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);
        
        mycon.getData(data);
/*        for (int i=0, tmp=1; i<8; i++) {
            pw = pow((float)2,i);
            b[i] = (int)((data[0] & tmp)/pw);
            pc.printf("%d ", b[i]);
            tmp *= 2;
        }
        for (int i=8, tmp=1, j=0; i<16; i++, j++) {
            pw = pow((float)2,j);
            b[i] = (int)((data[1] & tmp)/pw);
            pc.printf("%d ", b[i]);
            tmp *= 2;
        }
*/
        for (int i=0, tmp=1; i<8; i++) {
            pw = pow((float)2,i);
            b[i] = (int)((data[0] & tmp)/pw);
//          pc.printf("%d ", b[i]);          上のポインタの式が分からんので無理やり10進数に変える
            sum_1 += b[i]*pow((float)2,i+1);
            tmp *= 2;
        }
        pc.printf("%3d ",sum_1);
        /*初期化*/
        sum_1 = 0;
        
        for (int i=8, tmp=1, j=0; i<16; i++, j++) {
            pw = pow((float)2,j);
            b[i] = (int)((data[1] & tmp)/pw);
//          pc.printf("%d ", b[i]);             
            sum_2 += b[i]*pow((float)2,i-7);
            tmp *= 2;
        }
        pc.printf("%3d ",sum_2);
        /*初期化*/
        sum_2 = 0;
        pc.printf(" | ");
        
        for (int i=0; i<4; i++) {
            stick[i] = (data[i+2] - 128)*(-1);
            pc.printf("%3d ", stick[i]);
        }
        pc.printf(" | ");
        
        for (int i=0; i<2; i++) {
            trigger[i] = data[i+6];
            pc.printf("%3d ", trigger[i]);
        }
        pc.printf(" | ");
        
        for (int i=0; i<3; i++) {
            volume[i] = data[i+9];
            pc.printf("%3d ", volume[i]);
        }
        pc.printf(" | ");
        
        for (int i=0; i<4; i++) {
            toggle[i] = data[i+12];
            pc.printf("%3d ", toggle[i]);
        }
        pc.printf(" | ");
        
        timeout = data[8];
        pc.printf("%3d ", timeout);
        pc.printf(" | ");
        
        
#endif
        if (mycon.getStatus()) pc.printf("receive");
        else pc.printf("anything error..."); 
        
        rcv.getData(pulse); //エンコーダ受信
        for(int i=0,j=0;i<4;i++,j+=2){
            angle[i] = pulse[j] << 8 | pulse[j+1];  
        }
        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
        if(abs(stick[2])<10){
            pc.printf("TA:%.2f pid:%.2f T-p:%.2f",TargetAngle,-angl.compute(),TargetAngle-posiTheta);
        }
        pc.printf("\r\n");
 
        //ブラシレスモーター
        pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
 
        pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
 
        pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
//        pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
//                  ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
        
        /*井上機構ON,OFF*/
        if(toggle[1]){
            speed[10] = -0.9;
        }else{
            speed[10] = 0;
        }
        if(toggle[1] && b[15]){
            speed[10] = 0.4;
        }
        
        if(toggle[3]){
            speed[11] = -0.9;
        }else{
            speed[11] = 0;
        }
        if(toggle[3] && b[15]){
            speed[11] = 0.4;
        }
        
        /*フジモン機構*/
        if(toggle[2]){
            speed[6] = 0.6;
            speed[7] = 0.6;   
        }else{
            speed[6] = 0;
            speed[7] = 0;            
        }
        
        /*展開昇降*/
        if(b[5] != 0){
            speed[4] = 0.5;
            speed[5] = 0.5;
        }else if(b[4] != 0){
            speed[4] = -0.35;
            speed[5] = -0.35;
        }else{
            speed[4] = 0;
            speed[5] = 0;
        }
        //機構昇降
        if(b[9]){
            speed[8] = -0.35;
        }else
        if(b[13]){    
            speed[8] = 0.4;
        }
        if(b[11]){
            speed[9] = -0.35;
        }else
        if(b[14]){    
            speed[9] = 0.4;
        }
        if((b[9]!=1) && (b[13]!=1)){
            speed[8]=0;
        }
        if((b[11]!=1) && (b[14]!=1)){
            speed[9]=0;
        }
 
        /*PID*/
        if(abs(stick[2]) < 10){
            if(fabs(TargetAngle-posiTheta)>8){
                TargetAngle += 15*pm(posiTheta-TargetAngle);
                if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
                    TargetAngle += -360*pm(TargetAngle);
                }
            }
            spin(TargetAngle);
        }
        /*全方位*/
        for(int i=0; i<4; i++){
            if(abs(stick[i]) > 10){
                engine[i] = maxSpeed*(stick[i]/128.0);
            }else if(b[0]){
                engine[1] = maxSpeed*(trigger[1]/225.0);
                engine[0] = 0;
            }else if(b[1]){
                engine[0] = -maxSpeed*(trigger[1]/225.0);
                engine[1] = 0;
            }else if(b[2]){
                engine[1] = -maxSpeed*(trigger[1]/225.0);
                engine[0] = 0;
            }else if(b[3]){
                engine[0] = maxSpeed*(trigger[1]/255.0);
                engine[1] = 0;
            }else{
                engine[i] = 0;
            }
        }
        /*旋回*/
        if(abs(stick[2]) > 10){
            spin_power = engine[2];
        }else{
            spin_power = angl.compute();
        }
        
        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<12; i++) motor[i].setSpeed(speed[i]);
        
}
void spin(double ang)
{
    angl.setSetPoint(ang);
    posiTheta = posi.getTheta()*(180.0/M_PI);
    angl.setProcessValue(posiTheta);
    //for(int i=4; i<12; i++) motor[i].setSpeed(0);旋回時以外はPID判定なのでsetSpeed(0)だと機構が動かなくなるので多分このコードいらないです。
    //pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta);
  
}
int pm(double num){
    return((num>0)-(num<0));
}