File content as of revision 19:6387e3f02b37:

//中央

#include "mbed.h"
#define WAIT_LOOP_TIME 0.001
#define YOKUTAN_DATAS_NUM 7
#define INPUT_DATAS_NUM 5
#define SEND_DATAS_CAN_ID 100
#define SEND_DATAS_TIME 0.5
#define THRESHOLD_OH_VALUE 0.14
#define SUM_UP_NUM 20
#define SAMPLE_INTERVAL 0.05

#define PITCH_INPUT_NEUTRAL 0.468 //1って書いた方
#define PITCH_INPUT_MAX 0.884
#define PITCH_INPUT_MIN 0.110

#define OLD_ROLL_NEUTRAL 0.739
#define ROLL_INPUT_NEUTRAL 0.468 //２て書いた方
#define ROLL_INPUT_MAX 0.963 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)
#define ROLL_INPUT_MIN 0.555 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)

#define SUMED_INPUT_NEUTRAL 0.457
#define SUMED_INPUT_MAX (PITCH_INPUT_MAX +ROLL_INPUT_MAX ) / 2.0                  //0.7825
#define SUMED_INPUT_MIN (PITCH_INPUT_MIN +ROLL_INPUT_MIN ) / 2.0

#define DEC_INPUT_NEUTRAL 0
#define DEC_INPUT_MAX (PITCH_INPUT_MAX -ROLL_INPUT_MIN ) / 2.0
#define DEC_INPUT_MIN (PITCH_INPUT_MIN -ROLL_INPUT_MAX ) / 2.0
#define PHASE_NUM 7 //奇数にしてください
/*
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(p20);
DigitalIn drug_R(p14);
DigitalIn drug_L(p19);
DigitalOut myled1(LED1);
DigitalOut myled2(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];
bool stateP = true;
bool stateQ = true;
bool stateP_old = true;
bool stateQ_old = true;

CANMessage recmsg_R;
CANMessage recmsg_L;

float get_analogin_ave(AnalogIn pin)
{
    float val = 0;
    for(int i = 0; i<SUM_UP_NUM; i++) {
        val += pin.read();
    }
    return val/SUM_UP_NUM;
}

//値を0から1の範囲に収めます
float FormatSumed2Range(float value)
{
    float result;
    if(value > 1)
        result = 1;
    else if(value < 0)
        result = 0;
    else
        result  = value;
    return result;
}

float FormatEach2Range(float value,float max,float min)
{
    float result;
    if(value > max)
        result= max;
    else if(value < min)
        result = min;
    else
        result = value;
    return result;
}

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

//Format2Rangeの後に呼び出してください
int SampleFloat(float f)
{
    int temp = ((f + 0.025) * 100.0) / 5;
    float result = temp / 20.0;
    return result;

}

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 0;
    else if(value>=max)
        return PHASE_NUM;

}


void InputControlValues()
{
    pc.printf("Roll:%f      Pitch:%f    ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL),pitchPin.read());
    float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL));
    float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read());
    float FormatedEachRollR = FormatEach2Range((MatchedPitch +MatchedRoll ) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
    float FormatSumedR =  FormatSumed2Range(FormatedEachRollR);
    *(int *)inputDatas_R =PhaseFloat(FormatSumedR,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
    pc.printf("FormatR:%f    ",FormatSumedR);
    float FormatedEachRollL = FormatEach2Range((MatchedPitch - MatchedRoll) / 2.0,DEC_INPUT_MAX,DEC_INPUT_MIN);
    float FormatSumedL =  FormatSumed2Range(FormatedEachRollL);
    pc.printf("FormatL:%f    ",FormatSumedL);
    *(int *)inputDatas_L = PhaseFloat(FormatSumedR,DEC_INPUT_MAX,DEC_INPUT_MIN);

    if(*(int *)inputDatas_R < 0)
        *(int *)inputDatas_R = 0;
    else if(*(int *)inputDatas_R > PHASE_NUM)
        *(int *)inputDatas_R = PHASE_NUM;
    if(*(int *)inputDatas_L < 0)
        *(int *)inputDatas_L = 0 ;
    else if(*(int *)inputDatas_L > PHASE_NUM)
        *(int *)inputDatas_L =PHASE_NUM;

    pc.printf("input_R:%d   input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L);
    inputDatas_R[4] = (char)drug_R;
    led4 =! led4;
    pc.printf("%c",*(char *)inputDatas_R[4]);
    //pc.printf("%c",(char)drug_R);
    inputDatas_L[4] = (char)drug_L;
    // pc.printf("");
}



void toString_R()
{
    pc.printf("R:");
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        pc.printf("%d:%i    ",i,inputDatas_R[i]);
    }
}

void toString_L()
{
    pc.printf("L:");
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        pc.printf("%d:%i    ",i,inputDatas_L[i]);
    }
    pc.printf("\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]);
    }
//    toKeikiSerial.putc(cadence);
// pc.printf("test\n\r");
}

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]);
        }
        myled1 = !myled1;
    }
    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]);
        }
        myled2 = !myled2;
    }
}

int main()
{
    init();
    while(1) {
        InputControlValues();
        wait_us(5);
        receiveDatas();
        SendDatas();
        //     pc.printf("%d",drug_R.read());
//        toString_R();
//        toString_L();
        wait(WAIT_LOOP_TIME);
    }
}