File content as of revision 59:04c26e2c20bb:

//中央
/*
******************************************
**magic character of debug**
*
*(0)]:pilot's input datas: inputR,inputL,normedPitch,normedRoll
                            ,rollIC.shiftedMax,rollIC.shiftedMin,(int)drug_R,(int)drug_L
*(1)g:getting datas: mpu(LR),servoV(LR) <- mbed always send this to pc(rsp)
*(2)_:data to debug what you want(joker)
******************************************
*
*/

#include "mbed.h"
#include "InputHandler.h"
//#include "rtos.h"

#define WAIT_LOOP_TIME 0.001
#define YOKUTAN_DATAS_NUM 7
#define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
#define SEND_DATAS_CAN_ID 0x700//強い0x0>>0x7ff弱い
#define SEND_DATAS_TIME 0.5
#define THRESHOLD_OH_VALUE 0.14
#define SAMPLE_INTERVAL 0.05

#define PITCH_PER_ROLL 1.0//pitchがrollの何倍影響するかを示す値
#define AVE_NUM 100


/*
roll入力とピッチ入力だとピッチの方が1.5倍効くように
*/
#define SIGN(x) (((x)>0)-((x)<0))

#define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0)
#define INPUT_DATA_DEBUG_FLAG debugflag[0].flag
#define TO_KEIKI_DATA_DEBUG_FLAG debugflag[1].flag
#define DEBUG_FLAG debugflag[2].flag

struct flaglist{
    char key;
    bool flag;
};
struct flaglist debugflag[]={
    {']',0},
    {'g',0},
    {'_',0},
    {'0',0}//footer
};
//-----------------------------------(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);// right hand
DigitalIn drug_R(p14);// right hand
AnalogIn pitchPin(p18);// left hand
DigitalIn drug_L(p17);// left hand
DigitalIn servoOff(p26);
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+1];//for \0 space
char inputDatas_L[INPUT_DATAS_NUM+1];
char toKeikiBufInput[(3+1)*2];
CANMessage recmsg_R(0x701,yokutanDatas_R,YOKUTAN_DATAS_NUM);
CANMessage recmsg_L(0x701,yokutanDatas_L,YOKUTAN_DATAS_NUM);
int to_keiki_sending_timer = 0;

void receiveFromPc(){
    while(pc.readable()){
        char c = pc.getc();
        for(int i = 0; debugflag[i].key != '0'; i++){
                if(debugflag[i].key == c)
                    debugflag[i].flag = !(debugflag[i].flag);
        }
    }
}

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

void InputControlValues()
{
    double normedPitch = pitchIC.Processing(pitchPin.read());
    double normedRoll = rollIC.Processing(rollPin.read());
    
    float inputR = 0.0;
    float inputL = 0.0;
    
    uint8_t soff = servoOff;
    uint8_t drR = drug_R;
    uint8_t drL = drug_L;
    drR = drR<<1 | (soff&0x00000001);
    drL = drL<<1 | (soff&0x00000001);
    //dr= 3->drug on and servo off, 2->drug on and servo on, 1->drug off and servo off, 0->drug off and servo on
    
    SyntheRollAndPitch(normedPitch,normedRoll,&inputR,&inputL);
    // adjust inputR/L (-0.99~0.99)
    if(abs(inputR)==1.0){
        inputR -= 0.01*SIGN(inputR);
    }
    if(abs(inputL)==1.0){
        inputL -= 0.01*SIGN(inputL);
    }
    sprintf(inputDatas_R,"%03d%1d",(int)(inputR*99),drR);
    sprintf(inputDatas_L,"%03d%1d",(int)(inputL*99),drL);
    sprintf(toKeikiBufInput,"%03d%1d,%03d%d",(int)(normedRoll*99),drR,(int)(normedPitch*99),drL);
    print2pc(INPUT_DATA_DEBUG_FLAG,"inR:%5.2f inL:%5.2f nrmR:%5.2f nrmL:%5.2f sMax:%5.2f sMin:%5.2f drugR:%d drugL:%d\n"
        ,inputR,inputL,normedPitch,normedRoll,rollIC.shiftedMax,rollIC.shiftedMin,(int)drug_R,(int)drug_L);

    //inputDatas_R[sizeof(float)] = (char)drug_R;
    //inputDatas_L[sizeof(float)] = (char)drug_L;
    // for(int i = 0; i< 7; i++)
//        pc.printf("%c",inputDatas_R[i]);
// pc.printf("\n\r");
    //pc.printf("%d",(int)drug_R);

    led4 =! led4;
}

void SendDatas()
{
    /*can*/
    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));
    
    /*keiki*/
    char *toKeikiBuf = (char*)malloc(sizeof(char)*55);
    int ahead = 0;
//    ahead += sprintf(toKeikiBuf,"i%s,%s",inputDatas_R,inputDatas_L);
    ahead += sprintf(toKeikiBuf,"i%s",toKeikiBufInput);
    if(can_R.read(recmsg_R)){
        led3 = !led3;
        char mpu1[3]={recmsg_R.data[0],recmsg_R.data[1],recmsg_R.data[2]};
        char mpu2[3]={recmsg_R.data[3],recmsg_R.data[4],recmsg_R.data[5]};
        char v = recmsg_R.data[6];
        ahead += sprintf(toKeikiBuf+ahead,",R%d,%d,%d",*(int*)mpu1,*(int*)mpu2,(int)v);
        TO_KEIKI_DATA_DEBUG_FLAG = 1;
    }
    if(can_L.read(recmsg_L)){
        led2 = !led2;
        char mpu1[4]={recmsg_L.data[0],recmsg_L.data[1],recmsg_L.data[2],(recmsg_L.data[2]>>7 == 0)?0:0xff};
        char mpu2[4]={recmsg_L.data[3],recmsg_L.data[4],recmsg_L.data[5],(recmsg_L.data[5]>>7 == 0)?0:0xff};
        char v = recmsg_L.data[6];
        ahead += sprintf(toKeikiBuf+ahead,",L%d,%d,%d",*(int32_t*)mpu1,*(int32_t*)mpu2,(int)v);
        TO_KEIKI_DATA_DEBUG_FLAG = 1;
    }
    sprintf(toKeikiBuf+ahead,"\n\r");
    pc.puts(toKeikiBuf);
    
    free(toKeikiBuf);
    
}

void init()
{
    servoOff.mode(PullDown);
    for(int i = 0; i < INPUT_DATAS_NUM; i++) {
        inputDatas_R[i] = 0;
        inputDatas_L[i] = 0;
    }
}



int main()
{
//    Thread tickFusokuThread(&tickFusoku);
    init();
    pitchIC.setNeutral(pitchPin.read());
    rollIC.setNeutral(rollPin.read());
    while(1) {
        receiveFromPc();
        InputControlValues();
        wait_us(5);
        SendDatas();
        wait(WAIT_LOOP_TIME);
    }
}