File content as of revision 50:b582ed334d3a:

//中央

#include "mbed.h"
#include "InputHandler.h"
//#include "rtos.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 PITCH_PER_ROLL 1.5//pitchがrollの何倍影響するかを示す値
#define AVE_NUM 1000

/*
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);
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);
Serial pc(USBTX,USBRX);
InputCalc pitchIC;
InputCalc rollIC;
//DigitalOut fusokuControl(p25);
//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];

CANMessage recmsg_R;
CANMessage recmsg_L;

//void tickFusoku(void const * arg){
//    while(1){
//        fusokuControl = 1;
//        wait_us(500);
//        fusokuControl = 0;
//        wait_us(4500);
//    }
//}
void SyntheRollAndPitch(double pitch,double roll,float* inputR,float* inputL)
{
    *inputR =  (pitch * PITCH_PER_ROLL + roll)/(PITCH_PER_ROLL + 1.0);
    *inputL =  (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
}
double GetAve(double input)
{
    float sum = 0.0;
    for (int i = 0; i < AVE_NUM; i++) {
        sum += input;
    }
    return sum / AVE_NUM;
}

void InputControlValues()
{
    double pitch =GetAve( pitchPin.read());
    double roll = GetAve(rollPin.read());
    double normedPitch = pitchIC.Processing(pitch);
    double normedRoll = rollIC.Processing(rollPin.read());
    // pc.printf("pitch:%4.3f  roll:%4.3f\n\r",normedPitch,normedRoll);
    SyntheRollAndPitch(normedPitch,normedRoll,(float*)inputDatas_R,(float *)inputDatas_L);
    pc.printf("R:%8.8f  L:%8.8f\n\r",*(float*)inputDatas_R,*(float *)inputDatas_L);
    // pc.printf("R:%d     L:%d    show Both Right %d %d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L,drug_R.read(), drug_L.read());
    inputDatas_R[sizeof(float)+ 2] = (char)drug_R;
    inputDatas_L[sizeof(float)+2] = (char)drug_L;
    led4 =! led4;
}

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]);

    }
    for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
        toKeikiSerial.putc(yokutanDatas_L[i]);
    }
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        toKeikiSerial.putc(inputDatas_R[i]);
    }
    for(int i = 0; i < INPUT_DATAS_NUM; 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];
        }
        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;
        }
        led2 = !led2;
    }
}

int main()
{
//    Thread tickFusokuThread(&tickFusoku);
    init();
    pitchIC.setNeutral(pitchPin.read());
    rollIC.setNeutral(rollPin.read());
    while(1) {

        InputControlValues();
        wait_us(5);
        receiveDatas();
        SendDatas();
        wait(WAIT_LOOP_TIME);
    }
}