albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2017-04-21
- Revision:
- 40:4455b8c02e46
- Parent:
- 39:9b31e207c75d
- Parent:
- 38:5bccc1c7f812
File content as of revision 40:4455b8c02e46:
//中央 #include "mbed.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 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); 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]; 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 tickFusoku(void const * arg){ while(1){ fusokuControl = 1; wait_us(5); fusokuControl = 0; wait_ms(2); } } 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 ",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 ",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("input_R:%d input_L:%d\n\r",*(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]); } 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]; // 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() { Thread tickFusokuThread(&tickFusoku); init(); setNeutral(); while(1) { InputControlValues(); wait_us(5); receiveDatas(); SendDatas(); wait(WAIT_LOOP_TIME); } }