File content as of revision 32:60af346703d3:

//中央

#include "mbed.h"
#define WAIT_LOOP_TIME 0.001
#define YOKUTAN_DATAS_NUM 7
#define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
#define SEND_DATAS_CAN_ID 100
#define SEND_DATAS_TIME 0.5
#define THRESHOLD_OH_VALUE 0.14
#define SAMPLE_INTERVAL 0.05

#define PHASE_NUM 13 //奇数にしてください
#define SUM_UP_NUM 10.0
#define PITCHPERROLL 1.5
/*
roll入力とピッチ入力だとピッチの方が1.5倍効くように
*/

//-----------------------------------(resetInterrupt def)
extern "C" void mbed_reset();
InterruptIn resetPin(p22);
Timer resetTimeCount;
void resetInterrupt()
{
    while(resetPin) {
        resetTimeCount.start();
        if(resetTimeCount.read()>3) mbed_reset();
    }
    resetTimeCount.reset();
}
//-------------------------------------------------------

CAN can_R(p30,p29);
CAN can_L(p9,p10);
Serial toKeikiSerial(p28,p27);
Serial pc(USBTX,USBRX);
AnalogIn rollPin(p15);
AnalogIn pitchPin(p18);
DigitalIn drug_R(p14);
DigitalIn drug_L(p17);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led4(LED4);
DigitalOut led3(LED3);
//Ticker sendDatasTicker;

char yokutanDatas_R[YOKUTAN_DATAS_NUM];
char yokutanDatas_L[YOKUTAN_DATAS_NUM];
char inputDatas_R[INPUT_DATAS_NUM];
char inputDatas_L[INPUT_DATAS_NUM];

float rollNeutral = 0.739;
float rollUpperDiff = 0;
float rollLowerDiff = 0;

float pitchNeutral = 0.468 ;//1って書いた方
float pitchUpperDiff = 0;
float pitchLowerDiff = 0;
float neutralDiff;

CANMessage recmsg_R;
CANMessage recmsg_L;

enum InputType {
    enumRoll,
    enumPitch
};

void setNeutral()
{
    float rollSum;
    float pitchSum;

    for(int i = 0; i < SUM_UP_NUM; i++) {
        rollSum += rollPin.read();
        pitchSum += pitchPin.read();
    }
    rollNeutral = rollSum / SUM_UP_NUM;
    pitchNeutral = pitchSum / SUM_UP_NUM;

    neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
    rollNeutral += neutralDiff;
    rollUpperDiff = 0;
}

void setMaxAndMin(InputType it,float value)
{
    if(it == enumPitch) {
        if(value >pitchNeutral + pitchUpperDiff)
            pitchUpperDiff = value - pitchNeutral;
        else if(value < rollNeutral + pitchLowerDiff)
            pitchLowerDiff = value - pitchNeutral;
        return;
    } else if(it == enumRoll) {
        if(value >rollNeutral + rollUpperDiff)
            rollUpperDiff = value - rollNeutral;
        else if(value < rollNeutral + rollLowerDiff)
            rollLowerDiff = value  -rollNeutral ;
        return;
    }
}

//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
float MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
{
    float Upper = max- neutral;
    float Lower  = neutral - min;

    if(it == enumRoll) {
        if(Upper > Lower) {
            if(value < neutral) {
                value =  neutral + ((value - neutral) * (Upper / Lower));
                rollLowerDiff  =  -rollUpperDiff; 
            }
        } else  {
            if(value > neutral) {
                value =  neutral + ((value - neutral) * (Lower / Upper));
                rollUpperDiff = -rollLowerDiff;
            }
        }
        return value;
    } else if(it == enumPitch) {
        if(Upper > Lower) {
            if(value < neutral) {
                value =  neutral + ((value -neutral) * (Upper / Lower));
                pitchLowerDiff =  -pitchUpperDiff;
            }
        } else  {
            if(value > neutral) {
                value =  neutral + ((value - neutral) * (Lower / Upper));
                pitchUpperDiff= -pitchLowerDiff;
            }
        }
        return value;
    } else return value;
}

//範囲外に値がない場合にエラーが発生するので範囲内に収める
float Format2Range(float value,float max,float min)
{
    float result;

    if(value > max)
        result= max;
    else if(value < min)
        result = min;
    else
        result = value;
    return result;
}

//値をint型の段階に分ける
int PhaseFloat(float value,float max,float min)
{
    float PhaseWidth = (max - min) / PHASE_NUM;
    if(value< max&& value > min) {
        for(int i = 1; i <= PHASE_NUM; i++) {
            if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
                return i;
        }
    } else  if(value <= min)
        return 1;
    else if(value>=max)
        return PHASE_NUM;
}

float SetRollPitchRacio(float pitch,float roll)
{
    return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
}

void InputControlValues()
{
    setMaxAndMin(enumRoll, rollPin.read());
    setMaxAndMin(enumPitch, pitchPin.read());

//   pc.printf("rollN:%f     rollMax:%f     rollMin:%f   pitchN:%f   pitchMax:%f   pitchUpper:%f  pitchMin:%f   pitchLo:%f  raw:%f\r\n",rollNeutral,rollNeutral+rollUpperDiff,rollNeutral+rollLowerDiff,pitchNeutral,pitchNeutral+pitchUpperDiff,pitchUpperDiff,pitchNeutral+pitchLowerDiff,pitchLowerDiff,pitchPin.read());
    float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
    float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
//pc.printf("Rollmax:%f   value:%f    Min:%f" ,);
    float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
    *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));

    float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral  +pitchLowerDiff, - rollNeutral - rollUpperDiff));
    //pc.printf("matched:%f    max:%f  min:%f\n\r",(MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral  +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0);
    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
    //pc.printf("Format:%f    max:%f  min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
    if(*(int *)inputDatas_R < 1)
        *(int *)inputDatas_R = 1;
    else if(*(int *)inputDatas_R > PHASE_NUM)
        *(int *)inputDatas_R   = PHASE_NUM;
    if(*(int *)inputDatas_L < 1)
        *(int *)inputDatas_L = 1 ;
    else if(*(int *)inputDatas_L > PHASE_NUM)
        *(int *)inputDatas_L =PHASE_NUM;

    pc.printf("DR :%d   DL:%d     input_R:%d   input_L:%d\n\r",drug_R.read(),drug_L.read(),*(int *)inputDatas_R,*(int *)inputDatas_L);
    inputDatas_R[sizeof(int)+ 2] = (char)drug_R;
    led4 =! led4;
    // pc.printf("%c",*(char *)inputDatas_R[4]);
    //pc.printf("%c",(char)drug_R);
    inputDatas_L[sizeof(int)+2] = (char)drug_L;
  //  pc.printf("eruron:%f    drug:%f\n\r,",);
}

void SendDatas()
{
    can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
    can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
    toKeikiSerial.putc(';');
    for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {

        toKeikiSerial.putc(yokutanDatas_R[i]);
        toKeikiSerial.putc(yokutanDatas_L[i]);
    }
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        toKeikiSerial.putc(inputDatas_R[i]);
        toKeikiSerial.putc(inputDatas_L[i]);
    }
}

void init()
{
//--------------------------------------(resetInterrupt init)
    resetPin.rise(resetInterrupt);
    resetPin.mode(PullDown);
//-----------------------------------------------------------
//    sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        inputDatas_R[i] = 0;
        inputDatas_L[i] = 0;
    }
}

void receiveDatas()
{
    if(can_R.read(recmsg_R)) {
        for(int i = 0; i < recmsg_R.len; i++) {
            yokutanDatas_R[i] = recmsg_R.data[i];
            //   pc.printf("%c",yokutanDatas_R[i]);
        }
        led1 = !led1;
    }
    if(can_L.read(recmsg_L)) {
        for(int i = 0; i < recmsg_L.len; i++) {
            yokutanDatas_L[i] = recmsg_L.data[i];
            led3 = !led3;
            //  pc.printf("%c",yokutanDatas_L[i]);
        }
        led2 = !led2;
    }
}

int main()
{
    init();
    setNeutral();
    while(1) {
        InputControlValues();
        wait_us(5);
        receiveDatas();
        SendDatas();
        wait(WAIT_LOOP_TIME);
    }
}